HiveCoreRD/hivecore_robot_motion
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

refactor robot_control package

Browse Source
This commit is contained in:
hivecore
2026-01-13 18:00:09 +08:00
parent 541f0dbd02
commit d2824e1657
28 changed files with 1076 additions and 991 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
src/robot_control/CMakeLists.txt
View File

@@ -37,6 +37,7 @@ set(SOURCES
src/core/robot_control_manager.cpp
src/core/controller_factory.cpp
src/core/remote_controller.cpp
src/core/motion_parameters.cpp
src/actions/action_manager.cpp
src/controllers/control_base.cpp
src/controllers/leg_control.cpp

4
src/robot_control/include/actions/action_manager.hpp
View File

@@ -1,8 +1,8 @@
/**
* @file action_manager.hpp
* @brief Action管理器 - 统一管理所有ROS 2 Action服务器
* @author Robot Control Team
* @date 2024
* @author Ray
* @date 2025
*
* 该类负责：
* - 创建和管理所有Action服务器（MoveHome, MoveLeg, MoveWaist, MoveWheel）

28
src/robot_control/include/controllers/arm_control.hpp
View File

@@ -1,8 +1,8 @@
/**
* @file arm_control.hpp
* @brief 手臂控制器 - 控制机器人手臂运动
* @author Robot Control Team
* @date 2024
* @author Ray
* @date 2025
*/
#pragma once
@@ -39,26 +39,14 @@ namespace robot_control
public:
/**
* @brief 构造函数
* @param total_joints 关节总数（每个臂6个关节，总共12个）
* @param lengthParameters 长度参数（从MotionParameters中的left_arm_length_或right_arm_length_获取）
* @param maxSpeed 最大速度（度/秒）
* @param maxAcc 最大加速度（度/秒²）
* @param minLimits 最小限位（度）
* @param maxLimits 最大限位（度）
* @param home_positions 回零位置（弧度）
* @param zero_positions 零位置（弧度）
* @param joint_directions 关节方向
* @param joint_names 关节名称列表（左臂+右臂）
* @param motion_params MotionParameters 引用，用于获取 JointInfo
* @param lengthParameters 长度参数（从MotionParameters中的dual_arm_length_获取，包含左右臂）
*/
ArmControl(
size_t total_joints,
const std::vector<double>& lengthParameters,
const std::vector<double>& maxSpeed,
const std::vector<double>& maxAcc,
const std::vector<double>& minLimits,
const std::vector<double>& maxLimits,
const std::vector<double>& home_positions,
const std::vector<double>& zero_positions,
const std::vector<int>& joint_directions);
const std::vector<std::string>& joint_names,
const std::map<std::string, JointInfo>& joints_info_map,
const std::vector<double>& lengthParameters);
/**
* @brief 析构函数

50
src/robot_control/include/controllers/control_base.hpp
View File

@@ -1,8 +1,8 @@
/**
* @file control_base.hpp
* @brief 控制器基类 - 所有控制器的基类
* @author Robot Control Team
* @date 2024
* @author Ray
* @date 2025
*
* 该类定义了所有控制器的通用接口和功能，包括：
* - 关节位置、速度、加速度管理
@@ -15,8 +15,10 @@
#include <vector>
#include <memory>
#include <string>
#include "utils/trapezoidal_trajectory.hpp"
#include "utils/s_curve_trajectory.hpp"
#include "core/motion_parameters.hpp"
namespace robot_control
{
@@ -38,27 +40,15 @@ namespace robot_control
{
public:
/**
* @brief 构造函数
* @param total_joints 关节总数
* @brief 构造函数（新版本，使用 JointInfo）
* @param joint_names 关节名称列表
* @param joints_info_map 关节信息字典（只包含与当前控制器相关的关节）
* @param lengthParameters 长度参数
* @param maxSpeed 最大速度
* @param maxAcc 最大加速度
* @param minLimits 最小限位
* @param maxLimits 最大限位
* @param home_positions 回零位置
* @param zero_positions 零位置
* @param joint_directions 关节方向
*/
ControlBase(
size_t total_joints,
const std::vector<double>& lengthParameters,
const std::vector<double>& maxSpeed,
const std::vector<double>& maxAcc,
const std::vector<double>& minLimits,
const std::vector<double>& maxLimits,
const std::vector<double>& home_positions,
const std::vector<double>& zero_positions,
const std::vector<int>& joint_directions);
const std::vector<std::string>& joint_names,
const std::map<std::string, JointInfo>& joints_info_map,
const std::vector<double>& lengthParameters);
/**
* @brief 虚析构函数
@@ -144,21 +134,10 @@ namespace robot_control
protected:
size_t total_joints_; ///< 关节总数
std::vector<double> joint_position_; ///< 当前关节位置
std::vector<double> joint_velocity_; ///< 当前关节速度
std::vector<double> joint_acceleration_; ///< 当前关节加速度
std::vector<double> joint_torque_; ///< 当前关节力矩
std::vector<double> joint_commands_; ///< 关节命令
std::vector<double> joint_position_desired_; ///< 期望关节位置
std::vector<double> joint_home_positions_; ///< 回零位置
std::vector<double> joint_min_limits_; ///< 最小限位
std::vector<double> joint_max_limits_; ///< 最大限位
std::vector<JointInfo> joints_info_; ///< 关节信息列表（包含限位、速度、加速度、home位置、当前状态、指令等）
// 通用参数（所有控制器共享）
std::vector<double> lengthParameters_; ///< 长度参数
std::vector<int> joint_directions_; ///< 关节方向
std::vector<double> joint_zero_positions_; ///< 零点位置
bool is_target_set_; ///< 目标是否已设置
TrapezoidalTrajectory* trapezoidalTrajectory_; ///< 梯形轨迹规划器
@@ -173,5 +152,12 @@ namespace robot_control
bool is_joint_position_initialized_; ///< 关节位置是否已初始化
static constexpr double POSITION_TOLERANCE = 0.01; ///< 位置容差（弧度）
private:
/**
* @brief 从 joints_info_ 提取当前位置向量（供轨迹规划器使用）
*/
std::vector<double> getCurrentPositions() const;
};
}

30
src/robot_control/include/controllers/leg_control.hpp
View File

@@ -1,16 +1,19 @@
/**
* @file leg_control.hpp
* @brief 腿部控制器 - 控制机器人腿部运动
* @author Robot Control Team
* @date 2024
* @author Ray
* @date 2025
*/
#pragma once
#include "controllers/control_base.hpp"
#include <string>
namespace robot_control
{
// 前向声明
class MotionParameters;
/**
* @class LegControl
* @brief 腿部控制器类
@@ -22,26 +25,14 @@ namespace robot_control
public:
/**
* @brief 构造函数
* @param total_joints 关节总数
* @param joint_names 关节名称列表
* @param motion_params MotionParameters 引用，用于获取 JointInfo
* @param lengthParameters 长度参数
* @param maxSpeed 最大速度
* @param maxAcc 最大加速度
* @param minLimits 最小限位
* @param maxLimits 最大限位
* @param home_positions 回零位置
* @param zero_positions 零位置
* @param joint_directions 关节方向
*/
LegControl(
size_t total_joints,
const std::vector<double>& lengthParameters,
const std::vector<double>& maxSpeed,
const std::vector<double>& maxAcc,
const std::vector<double>& minLimits,
const std::vector<double>& maxLimits,
const std::vector<double>& home_positions,
const std::vector<double>& zero_positions,
const std::vector<int>& joint_directions);
const std::vector<std::string>& joint_names,
const std::map<std::string, JointInfo>& joints_info_map,
const std::vector<double>& lengthParameters);
/**
* @brief 析构函数
@@ -61,5 +52,6 @@ namespace robot_control
private:
double calculate_angle_from_links(double side1, double side2, double side_opposite, bool is_degree);
std::vector<double> target_joint_positions_; ///< 目标关节位置
};
}

30
src/robot_control/include/controllers/waist_control.hpp
View File

@@ -1,17 +1,20 @@
/**
* @file waist_control.hpp
* @brief 腰部控制器 - 控制机器人腰部运动
* @author Robot Control Team
* @date 2024
* @author Ray
* @date 2025
*/
#pragma once
#include "controllers/control_base.hpp"
#include <Eigen/Dense>
#include <string>
namespace robot_control
{
// 前向声明
class MotionParameters;
/**
* @class WaistControl
* @brief 腰部控制器类
@@ -23,26 +26,14 @@ namespace robot_control
public:
/**
* @brief 构造函数
* @param total_joints 关节总数
* @param joint_names 关节名称列表
* @param motion_params MotionParameters 引用，用于获取 JointInfo
* @param lengthParameters 长度参数
* @param maxSpeed 最大速度
* @param maxAcc 最大加速度
* @param minLimits 最小限位
* @param maxLimits 最大限位
* @param home_positions 回零位置
* @param zero_positions 零位置
* @param joint_directions 关节方向
*/
WaistControl(
size_t total_joints,
const std::vector<double>& lengthParameters,
const std::vector<double>& maxSpeed,
const std::vector<double>& maxAcc,
const std::vector<double>& minLimits,
const std::vector<double>& maxLimits,
const std::vector<double>& home_positions,
const std::vector<double>& zero_positions,
const std::vector<int>& joint_directions);
const std::vector<std::string>& joint_names,
const std::map<std::string, JointInfo>& joints_info_map,
const std::vector<double>& lengthParameters);
/**
* @brief 析构函数
@@ -65,5 +56,6 @@ namespace robot_control
bool is_cmd_send_finished_; ///< 命令是否已发送完成
int time_out_count_; ///< 超时计数
static constexpr int TIME_OUT_COUNT = 500; ///< 超时计数阈值
std::vector<double> target_joint_positions_; ///< 目标关节位置
};
}

30
src/robot_control/include/controllers/wheel_control.hpp
View File

@@ -1,17 +1,20 @@
/**
* @file wheel_control.hpp
* @brief 轮子控制器 - 控制机器人轮子运动
* @author Robot Control Team
* @date 2024
* @author Ray
* @date 2025
*/
#pragma once
#include "controllers/control_base.hpp"
#include <Eigen/Dense>
#include <string>
namespace robot_control
{
// 前向声明
class MotionParameters;
/**
* @class WheelControl
* @brief 轮子控制器类
@@ -23,26 +26,14 @@ namespace robot_control
public:
/**
* @brief 构造函数
* @param total_joints 关节总数
* @param joint_names 关节名称列表
* @param motion_params MotionParameters 引用，用于获取 JointInfo
* @param lengthParameters 长度参数
* @param maxSpeed 最大速度
* @param maxAcc 最大加速度
* @param minLimits 最小限位
* @param maxLimits 最大限位
* @param home_positions 回零位置
* @param zero_positions 零位置
* @param joint_directions 关节方向
*/
WheelControl(
size_t total_joints,
const std::vector<double>& lengthParameters,
const std::vector<double>& maxSpeed,
const std::vector<double>& maxAcc,
const std::vector<double>& minLimits,
const std::vector<double>& maxLimits,
const std::vector<double>& home_positions,
const std::vector<double>& zero_positions,
const std::vector<int>& joint_directions);
const std::vector<std::string>& joint_names,
const std::map<std::string, JointInfo>& joints_info_map,
const std::vector<double>& lengthParameters);
/**
* @brief 析构函数
@@ -70,5 +61,6 @@ namespace robot_control
// 成员变量（特定于轮子控制器）
double lastMoveDistance_; ///< 上次移动距离
double moveRatio_; ///< 移动比例
std::vector<double> target_joint_positions_; ///< 目标关节位置
};
}

4
src/robot_control/include/core/common_enum.hpp
View File

@@ -1,8 +1,8 @@
/**
* @file common_enum.hpp
* @brief 通用枚举定义
* @author Robot Control Team
* @date 2024
* @author Ray
* @date 2025
*
* 该文件包含所有机器人控制相关的通用枚举定义。
*/

4
src/robot_control/include/core/controller_factory.hpp
View File

@@ -1,8 +1,8 @@
/**
* @file controller_factory.hpp
* @brief 控制器工厂 - 用于创建和管理控制器实例
* @author Robot Control Team
* @date 2024
* @author Ray
* @date 2025
*
* 该类提供工厂模式来创建不同类型的控制器实例，
* 支持动态控制器加载和管理。

409
src/robot_control/include/core/motion_parameters.hpp
View File

@@ -1,8 +1,8 @@
/**
* @file motion_parameters.hpp
* @brief 运动参数类 - 定义机器人运动控制的所有参数
* @author Robot Control Team
* @date 2024
* @author Ray
* @date 2025
*
* 该类包含机器人运动控制所需的所有参数，包括：
* - 关节索引和方向
@@ -41,38 +41,62 @@ namespace robot_control
// ==================== 结构体定义 ====================
/**
* @struct JointLimit
* @brief 关节限制参数结构体
* @struct JointInfo
* @brief 关节信息结构体
*
* 包含关节的限制参数：最大值、最小值和限制类型。
* 包含关节的所有信息：限制、速度、加速度、初始位置、当前状态、指令等。
*/
struct JointLimit {
int index; ///< 关节索引
double max; ///< 关节运动范围上限（如位置最大角度）
double min; ///< 关节运动范围下限（如位置最小角度）
LimitType limit_type; ///< 限制类型（区分位置/速度/力矩）
struct JointInfo {
// 配置参数
double max_limit; ///< 关节运动范围上限（如位置最大角度）
double min_limit; ///< 关节运动范围下限（如位置最小角度）
LimitType limit_type; ///< 限制类型（区分位置/速度/力矩）
double max_velocity; ///< 关节最大速度（度/秒或rad/s）
double max_acceleration; ///< 关节最大加速度（度/秒²或rad/s²）
double home_position; ///< 关节初始位置（Home位置，度或弧度）
// 当前状态
double current_position; ///< 当前关节位置
double current_velocity; ///< 当前关节速度
double current_acceleration; ///< 当前关节加速度
double current_torque; ///< 当前关节力矩
// 指令参数
double command_position; ///< 指令关节位置
double command_velocity; ///< 指令关节速度
double command_acceleration; ///< 指令关节加速度
double command_torque; ///< 指令关节力矩
/**
* @brief 默认构造函数
* 初始化所有成员为默认值（避免未定义值）
* 初始化所有成员为默认值（状态和指令参数初始化为0）
*/
JointLimit() : index(0), max(0.0), min(0.0), limit_type(LimitType::POSITION) {}
JointInfo()
: max_limit(0.0), min_limit(0.0), limit_type(LimitType::POSITION),
max_velocity(0.0), max_acceleration(0.0), home_position(0.0),
current_position(0.0), current_velocity(0.0), current_acceleration(0.0), current_torque(0.0),
command_position(0.0), command_velocity(0.0), command_acceleration(0.0), command_torque(0.0) {}
/**
* @brief 带参构造函数
* @param i 关节索引
* @param max_val 最大值
* @param min_val 最小值
* @param max_lim 最大值
* @param min_lim 最小值
* @param type 限制类型
* @param max_vel 最大速度
* @param max_acc 最大加速度
* @param home_pos Home位置
*/
JointLimit(int i, double max_val, double min_val, LimitType type)
: index(i), max(max_val), min(min_val), limit_type(type)
JointInfo(double max_lim, double min_lim, LimitType type, double max_vel, double max_acc, double home_pos = 0.0)
: max_limit(max_lim), min_limit(min_lim), limit_type(type),
max_velocity(max_vel), max_acceleration(max_acc), home_position(home_pos),
current_position(0.0), current_velocity(0.0), current_acceleration(0.0), current_torque(0.0),
command_position(0.0), command_velocity(0.0), command_acceleration(0.0), command_torque(0.0)
{
// 如果最大值小于最小值，交换它们并输出警告
if (max < min)
if (max_limit < min_limit)
{
std::cerr << "[Warning] JointLimit: max (" << max << ") < min (" << min << "), swapping values!" << std::endl;
std::swap(max, min);
std::cerr << "[Warning] JointInfo: max_limit (" << max_limit << ") < min_limit (" << min_limit << "), swapping values!" << std::endl;
std::swap(max_limit, min_limit);
}
}
};
@@ -84,6 +108,7 @@ namespace robot_control
* @brief 运动参数类
*
* 包含机器人运动控制所需的所有参数配置。
* 支持基于关节名称的动态配置，适应不同的机器人配置（不同的关节组合）。
*/
class MotionParameters
{
@@ -98,314 +123,33 @@ namespace robot_control
* - 轮子控制参数
* - 关节传动比和分辨率
*/
MotionParameters()
{
// ==================== 结构参数 ====================
wheel_radius_ = 0.09; // 轮子半径（米）
wheel_separation_ = 0.55; // 轮距（米）
MotionParameters();
wheel_length_ = {
wheel_radius_,
wheel_separation_
};
// ==================== 尺寸参数 ====================
// 腿长参数（单位：米）
leg_length_ = {
0.66, // 后腿长
0.385, // 大腿长
0.362, // 小腿长
0.55, // 前腿髋部长度
0.4 // 前腿大腿长度
};
waist_length_ = {
0.1, // 腰部长度
};
// 手臂长度参数（单位：米，每个臂6个关节对应的连杆长度）
// 注意：这些参数主要用于传统运动学计算，MoveIt会从URDF/SRDF加载实际参数
left_arm_length_ = {
0.1, 0.2, 0.15, 0.1, 0.05, 0.02 // 默认值，实际应从URDF加载
};
right_arm_length_ = {
0.1, 0.2, 0.15, 0.1, 0.05, 0.02 // 默认值，实际应从URDF加载
};
// ==================== 速度参数 ====================
// 轮子速度参数
max_linear_speed_x_ = 100; // 最大线速度 X（m/s）
max_linear_speed_z_ = 10; // 最大线速度 Z（m/s）
max_angular_speed_z_ = 50; // 最大角速度 Z（rad/s）
// 关节速度参数（度/秒）
max_joint_velocity_ = {
30, 30, 50, 50, 50, 50, 50, 50
};
// 关节加速度参数（度/秒²）
max_joint_acceleration_ = {
80, 80, 100, 100, 100, 100, 100, 100
};
// ==================== 关节索引 ====================
wheel_joint_indices_ = {0, 1};
waist_joint_indices_ = {0, 1};
leg_joint_indices_ = {2, 3, 4, 5, 6, 7};
// 左臂关节索引（从索引9开始，6个关节）
left_arm_joint_indices_ = {9, 10, 11, 12, 13, 14};
// 右臂关节索引（从索引15开始，6个关节）
right_arm_joint_indices_ = {15, 16, 17, 18, 19, 20};
total_joints_ = 8;
// ==================== 关节方向 ====================
wheel_joint_directions_ = {1, -1};
waist_joint_directions_ = {1, 1};
leg_joint_directions_ = {-1, 1, 1, -1, -1, 1};
// 手臂关节方向（每个臂6个关节，默认都是1）
left_arm_joint_directions_ = {1, 1, 1, 1, 1, 1};
right_arm_joint_directions_ = {1, 1, 1, 1, 1, 1};
// ==================== 关节传动参数 ====================
joint_gear_ratio_ = {100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}; // 传动比
joint_resolution_ = {524288, 524288, 524288, 524288, 524288, 524288, 524288, 524288}; // 分辨率
// ==================== 关节限位 ====================
joint_limits_ = {
JointLimit(0, 40.0, -40.0, LimitType::POSITION),
JointLimit(1, 190.0, -190.0, LimitType::POSITION),
JointLimit(2, 0.0, -90.0, LimitType::POSITION),
JointLimit(3, 90.0, 0.0, LimitType::POSITION),
JointLimit(4, 60, 0.0, LimitType::POSITION),
JointLimit(5, 0.0, -90.0, LimitType::POSITION),
JointLimit(6, 0.0, -60.0, LimitType::POSITION),
JointLimit(7, 90.0, 0.0, LimitType::POSITION),
};
// ==================== 初始化限制参数 ====================
wheel_max_velocity_.resize(wheel_joint_indices_.size());
wheel_max_acceleration_.resize(wheel_joint_indices_.size());
wheel_max_limit_.resize(wheel_joint_indices_.size());
wheel_min_limit_.resize(wheel_joint_indices_.size());
wheel_max_velocity_ = {5, 5};
wheel_max_acceleration_ = {25, 25};
wheel_max_limit_ = {0.0, 0.0}; // 轮子无限位
wheel_min_limit_ = {0.0, 0.0}; // 轮子无限位
waist_min_limit_.resize(waist_joint_indices_.size());
waist_max_limit_.resize(waist_joint_indices_.size());
waist_max_velocity_.resize(waist_joint_indices_.size());
waist_max_acceleration_.resize(waist_joint_indices_.size());
leg_max_limit_.resize(leg_joint_indices_.size());
leg_min_limit_.resize(leg_joint_indices_.size());
leg_max_velocity_.resize(leg_joint_indices_.size());
leg_max_acceleration_.resize(leg_joint_indices_.size());
// 初始化手臂参数
left_arm_max_limit_.resize(left_arm_joint_indices_.size());
left_arm_min_limit_.resize(left_arm_joint_indices_.size());
left_arm_max_velocity_.resize(left_arm_joint_indices_.size());
left_arm_max_acceleration_.resize(left_arm_joint_indices_.size());
right_arm_max_limit_.resize(right_arm_joint_indices_.size());
right_arm_min_limit_.resize(right_arm_joint_indices_.size());
right_arm_max_velocity_.resize(right_arm_joint_indices_.size());
right_arm_max_acceleration_.resize(right_arm_joint_indices_.size());
// 从 joint_limits_ 中提取各部分的限位和速度参数
for (size_t i = 0; i < waist_joint_indices_.size(); i++)
{
waist_max_limit_[i] = joint_limits_[waist_joint_indices_[i]].max;
waist_min_limit_[i] = joint_limits_[waist_joint_indices_[i]].min;
waist_max_velocity_[i] = max_joint_velocity_[waist_joint_indices_[i]];
waist_max_acceleration_[i] = max_joint_acceleration_[waist_joint_indices_[i]];
}
for (size_t i = 0; i < leg_joint_indices_.size(); i++)
{
leg_max_limit_[i] = joint_limits_[leg_joint_indices_[i]].max;
leg_min_limit_[i] = joint_limits_[leg_joint_indices_[i]].min;
leg_max_velocity_[i] = max_joint_velocity_[leg_joint_indices_[i]];
leg_max_acceleration_[i] = max_joint_acceleration_[leg_joint_indices_[i]];
}
// 从 joint_limits_ 中提取手臂参数（注意：如果关节索引超出范围，使用默认值）
// 左臂参数（从索引9开始）
for (size_t i = 0; i < left_arm_joint_indices_.size(); i++)
{
int joint_idx = left_arm_joint_indices_[i];
if (joint_idx < static_cast<int>(joint_limits_.size()))
{
left_arm_max_limit_[i] = joint_limits_[joint_idx].max;
left_arm_min_limit_[i] = joint_limits_[joint_idx].min;
}
else
{
left_arm_max_limit_[i] = 180.0; // 默认值（度）
left_arm_min_limit_[i] = -180.0;
}
if (joint_idx < static_cast<int>(max_joint_velocity_.size()))
{
left_arm_max_velocity_[i] = max_joint_velocity_[joint_idx];
left_arm_max_acceleration_[i] = max_joint_acceleration_[joint_idx];
}
else
{
left_arm_max_velocity_[i] = 30.0; // 默认值（度/秒）
left_arm_max_acceleration_[i] = 80.0; // 默认值（度/秒²）
}
}
// 右臂参数（从索引15开始）
for (size_t i = 0; i < right_arm_joint_indices_.size(); i++)
{
int joint_idx = right_arm_joint_indices_[i];
if (joint_idx < static_cast<int>(joint_limits_.size()))
{
right_arm_max_limit_[i] = joint_limits_[joint_idx].max;
right_arm_min_limit_[i] = joint_limits_[joint_idx].min;
}
else
{
right_arm_max_limit_[i] = 180.0; // 默认值（度）
right_arm_min_limit_[i] = -180.0;
}
if (joint_idx < static_cast<int>(max_joint_velocity_.size()))
{
right_arm_max_velocity_[i] = max_joint_velocity_[joint_idx];
right_arm_max_acceleration_[i] = max_joint_acceleration_[joint_idx];
}
else
{
right_arm_max_velocity_[i] = 30.0; // 默认值（度/秒）
right_arm_max_acceleration_[i] = 80.0; // 默认值（度/秒²）
}
}
// ==================== 零点位置 ====================
waist_zero_positions_ = {
350.0,
261.82
};
leg_zero_positions_ = {
217.52,
120.84,
108.7,
221.95,
234.14,
125.39
};
wheel_zero_positions_ = {
0.0,
0.0
};
// 手臂零点位置（弧度，每个臂6个关节）
left_arm_zero_positions_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
right_arm_zero_positions_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
// ==================== 初始位置（Home位置） ====================
waist_home_positions_ = {
350.0,
261.82
};
// 后腿零点为站直时候的状态，然后 + 25度，后腿长 0.66m
// 前腿零点为站直时候的状态，然后髋部 pitch + 49度，大腿长 0.38m
// 膝盖 pitch 再收回 35.5 度，剩余 13.5度长度为 0.35m
leg_home_positions_ = {
217.52 - 65.0,
120.84 + 41.0,
108.7 + 40.63,
221.95 - 41.0,
234.14 - 29.504,
125.39 + 65.0
};
wheel_home_positions_ = {
0.0,
0.0
};
// 手臂Home位置（弧度，每个臂6个关节，默认与zero位置相同）
left_arm_home_positions_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
right_arm_home_positions_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
// ==================== 转换因子 ====================
pulse_to_degree_.resize(joint_resolution_.size());
degree_to_pulse_.resize(joint_resolution_.size());
for (size_t i = 0; i < joint_resolution_.size(); i++)
{
degree_to_pulse_[i] = joint_resolution_[i] / 360.0;
pulse_to_degree_[i] = 360.0 / joint_resolution_[i];
}
// ==================== Jog参数 ====================
jog_step_size_ = 10.0 / (1000.0 / CYCLE_TIME); // 5度每秒
}
/**
* @brief 从 JointState 消息初始化关节名称映射
* @param joint_names 关节名称列表（从 JointState.name 获取）
*
* 该方法根据传入的关节名称列表，建立名称到索引的映射，
* 并根据配置的关节组名称列表（wheel_joint_names_等）生成对应的索引。
* 同时从现有的基于索引的参数生成基于名称的映射。
*/
void InitializeJointNameMapping(const std::vector<std::string>& joint_names);
// ==================== 公共成员变量 ====================
// ---------- 关节索引和方向 ----------
// ---------- 基于名称的关节配置（主要使用） ----------
std::vector<std::string> all_joint_names_; ///< 所有关节名称列表（按顺序，从JointState消息中获取）
std::map<std::string, size_t> joint_name_to_index_; ///< 关节名称到索引的映射
std::map<std::string, JointInfo> joints_info_map_; ///< 关节信息字典（基于名称，包含限制、速度、加速度等信息）
// 关节组名称列表（用于配置哪些关节属于哪个控制器）
std::vector<std::string> wheel_joint_names_; ///< 轮子关节名称列表
std::vector<std::string> waist_joint_names_; ///< 腰部关节名称列表
std::vector<std::string> leg_joint_names_; ///< 腿部关节名称列表
std::vector<std::string> dual_arm_joint_names_; ///< 双臂关节名称列表（合并左右臂）
// ---------- 关节索引和方向（保留以保持兼容性，但将从名称映射生成） ----------
size_t total_joints_; ///< 总关节数
std::vector<int> leg_joint_indices_; ///< 腿部关节索引
std::vector<int> arm_joint_indices_; ///< 手臂关节索引（所有手臂关节）
std::vector<int> left_arm_joint_indices_; ///< 左臂关节索引
std::vector<int> right_arm_joint_indices_; ///< 右臂关节索引
std::vector<int> wheel_joint_indices_; ///< 轮子关节索引
std::vector<int> waist_joint_indices_; ///< 腰部关节索引
std::vector<int> wheel_joint_directions_; ///< 轮子关节方向
std::vector<int> waist_joint_directions_; ///< 腰部关节方向
std::vector<int> leg_joint_directions_; ///< 腿部关节方向
std::vector<int> left_arm_joint_directions_; ///< 左臂关节方向
std::vector<int> right_arm_joint_directions_; ///< 右臂关节方向
// ---------- 位置参数 ----------
std::vector<double> leg_home_positions_; ///< 腿部初始位置（Home位置）
std::vector<double> waist_home_positions_; ///< 腰部初始位置（Home位置）
std::vector<double> wheel_home_positions_; ///< 轮子初始位置（Home位置）
std::vector<double> left_arm_home_positions_; ///< 左臂初始位置（Home位置，弧度）
std::vector<double> right_arm_home_positions_; ///< 右臂初始位置（Home位置，弧度）
std::vector<double> waist_zero_positions_; ///< 腰部零点位置
std::vector<double> leg_zero_positions_; ///< 腿部零点位置
std::vector<double> wheel_zero_positions_; ///< 轮子零点位置
std::vector<double> left_arm_zero_positions_; ///< 左臂零点位置（弧度）
std::vector<double> right_arm_zero_positions_; ///< 右臂零点位置（弧度）
// ---------- 限位参数 ----------
std::vector<JointLimit> joint_limits_; ///< 关节限制（包含所有关节）
std::vector<double> waist_min_limit_; ///< 腰部最小限位
std::vector<double> waist_max_limit_; ///< 腰部最大限位
std::vector<double> leg_min_limit_; ///< 腿部最小限位
std::vector<double> leg_max_limit_; ///< 腿部最大限位
std::vector<double> wheel_min_limit_; ///< 轮子最小限位
std::vector<double> wheel_max_limit_; ///< 轮子最大限位
std::vector<double> left_arm_min_limit_; ///< 左臂最小限位（度）
std::vector<double> left_arm_max_limit_; ///< 左臂最大限位（度）
std::vector<double> right_arm_min_limit_; ///< 右臂最小限位（度）
std::vector<double> right_arm_max_limit_; ///< 右臂最大限位（度）
// ---------- 速度参数 ----------
std::vector<double> max_joint_velocity_; ///< 所有关节的最大速度
std::vector<double> waist_max_velocity_; ///< 腰部最大速度
std::vector<double> leg_max_velocity_; ///< 腿部最大速度
std::vector<double> wheel_max_velocity_; ///< 轮子最大速度
std::vector<double> left_arm_max_velocity_; ///< 左臂最大速度（度/秒）
std::vector<double> right_arm_max_velocity_; ///< 右臂最大速度（度/秒）
// ---------- 加速度参数 ----------
std::vector<double> max_joint_acceleration_; ///< 所有关节的最大加速度
std::vector<double> waist_max_acceleration_; ///< 腰部最大加速度
std::vector<double> leg_max_acceleration_; ///< 腿部最大加速度
std::vector<double> wheel_max_acceleration_; ///< 轮子最大加速度
std::vector<double> left_arm_max_acceleration_; ///< 左臂最大加速度（度/秒²）
std::vector<double> right_arm_max_acceleration_; ///< 右臂最大加速度（度/秒²）
// ---------- 轮子控制参数 ----------
double wheel_radius_; ///< 轮子半径（米）
@@ -415,19 +159,18 @@ namespace robot_control
double max_angular_speed_z_; ///< 最大角速度 Z（rad/s）
// ---------- 尺寸参数 ----------
std::vector<double> leg_length_; ///< 腿部长度参数（米）
std::vector<double> leg_length_; ///< 腿部长度参数（米）+
std::vector<double> waist_length_; ///< 腰部长度参数（米）
std::vector<double> wheel_length_; ///< 轮子长度参数（米）
std::vector<double> left_arm_length_; ///< 左臂长度参数（米）
std::vector<double> right_arm_length_; ///< 右臂长度参数（米）
std::vector<double> dual_arm_length_; ///< 双臂长度参数（米，合并左右臂）
// ---------- 关节传动参数 ----------
std::vector<double> joint_gear_ratio_; ///< 关节传动比
std::vector<double> joint_resolution_; ///< 关节分辨率（编码器分辨率）
std::vector<double> degree_to_pulse_; ///< 角度转脉冲的转换因子
std::vector<double> pulse_to_degree_; ///< 脉冲转角度的转换因子
// ---------- 手臂参数 ----------
size_t arm_dof_; ///< 每条手臂的自由度数（关节数）
// ---------- Jog参数 ----------
double jog_step_size_; ///< Jog步长（度）
// ---------- 控制器启用标志 ----------
bool leg_controller_enabled_; ///< 腿部控制器是否启用
bool wheel_controller_enabled_; ///< 轮子控制器是否启用
bool waist_controller_enabled_; ///< 腰部控制器是否启用
bool arm_controller_enabled_; ///< 手臂控制器是否启用
};
}

4
src/robot_control/include/core/remote_controller.hpp
View File

@@ -1,8 +1,8 @@
/**
* @file remote_controller.hpp
* @brief 遥控器控制器 - 处理遥控器指令
* @author Robot Control Team
* @date 2024
* @author Ray
* @date 2025
*
* 该类负责：
* - 处理遥控器命令（gamepad消息）

21
src/robot_control/include/core/robot_control_manager.hpp
View File

@@ -1,8 +1,8 @@
/**
* @file robot_control_manager.hpp
* @brief 机器人控制管理器 - 统一管理所有控制器（腿、轮、腰、臂）
* @author Robot Control Team
* @date 2024
* @author Ray
* @date 2025
*
* 该类负责：
* - 初始化和管理各个控制器（Leg, Wheel, Waist, Arm）
@@ -53,9 +53,8 @@ namespace robot_control
public:
/**
* @brief 构造函数
* @param enabled_controllers 启用的控制器列表，默认为 {"leg", "wheel", "waist"}
*/
explicit RobotControlManager(const std::vector<std::string>& enabled_controllers = {"leg", "wheel", "waist"});
explicit RobotControlManager();
/**
* @brief 析构函数
@@ -297,9 +296,8 @@ namespace robot_control
private:
/**
* @brief 初始化函数
* @param enabled_controllers 启用的控制器列表
*/
void init(const std::vector<std::string>& enabled_controllers);
void init();
/**
* @brief 分配临时值（从关节命令中提取各控制器对应的值）
@@ -311,6 +309,13 @@ namespace robot_control
*/
void UpdateJointCommands();
/**
* @brief 从关节名称列表获取索引列表（辅助函数）
* @param joint_names 关节名称列表
* @return 索引列表
*/
std::vector<size_t> GetJointIndicesFromNames(const std::vector<std::string>& joint_names) const;
// Quaternion conversion utilities (private helpers)
void QuaternionToRPYRad(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw);
void QuaternionToRPYDeg(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw);
@@ -342,10 +347,7 @@ namespace robot_control
std::vector<double> jointPositions_; ///< 关节位置（弧度）
std::vector<double> jointVelocities_; ///< 关节速度（弧度/秒）
std::vector<double> wheelVelocities_; ///< 轮子速度（弧度/秒）
std::vector<double> jointAccelerations_; ///< 关节加速度（弧度/秒^2）
std::vector<double> wheelAccelerations_; ///< 轮子加速度（弧度/秒^2）
std::vector<double> jointEfforts_; ///< 关节力矩
std::vector<double> wheelEfforts_; ///< 轮子力矩
// 关节命令和状态
std_msgs::msg::Float64MultiArray jointCommands_; ///< 关节命令
@@ -371,5 +373,6 @@ namespace robot_control
// 初始化相关
std::vector<bool> jointInited_; ///< 各关节是否已初始化
bool isJointInitValueSet_; ///< 关节初始化值是否已设置
bool joint_name_mapping_initialized_; ///< 关节名称映射是否已初始化
};
}

26
src/robot_control/include/core/robot_control_node.hpp
View File

@@ -1,8 +1,8 @@
/**
* @file robot_control_node.hpp
* @brief ROS 2 机器人控制节点 - 主控制节点
* @author Robot Control Team
* @date 2024
* @author Ray
* @date 2025
*
* 该类是ROS 2节点，负责：
* - 创建和管理ROS 2话题、服务和动作服务器
@@ -30,6 +30,7 @@
#include "interfaces/action/move_wheel.hpp"
#include "interfaces/msg/motor_cmd.hpp"
#include "interfaces/srv/motor_param.hpp"
#include "control_msgs/msg/dynamic_interface_group_values.hpp"
#define RECORD_FLAG 0 ///< 数据记录标志（0=禁用，1=启用）
@@ -90,6 +91,7 @@ namespace robot_control
// ==================== ROS 2 通信接口 ====================
rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr jointTrajectoryPub_; ///< 关节轨迹发布器
rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr gpioPub_; ///< GPIO发布器
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr joyCommandSub_; ///< 手柄命令订阅器
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr jointStatesSub_; ///< 关节状态订阅器
rclcpp::Publisher<MotorCmd>::SharedPtr motorCmdPub_; ///< 电机命令发布器
@@ -148,5 +150,25 @@ namespace robot_control
* @param msg IMU消息
*/
void ImuMsgCallback(const ImuMsg::SharedPtr msg);
/**
* @brief 激活指定名称的控制器
* @param controller_name 控制器名称
* @return 激活是否成功
*/
bool activateController(const std::string& controller_name);
/**
* @brief 重置所有电机故障
*/
void motor_reset_fault_all();
/**
* @brief 启用所有电机
* @param enable 使能标志（1为使能，0为禁用）
*/
void motor_enable_all(double enable);
};
}

3
src/robot_control/include/utils/s_curve_trajectory.hpp
View File

@@ -201,7 +201,8 @@ private:
std::vector<double> stop_start_pos_; // 急停起始位置
std::vector<double> stop_start_vel_; // 急停起始速度
std::vector<double> stop_start_acc_; // 急停起始加速度
std::vector<double> stop_jerk_; // 急停Jerk（各轴独立）
std::vector<double> stop_jerk_; // 急停Jerk（各轴独立，已不使用）
std::vector<double> stop_decel_; // 急停减速度（各轴独立，已调整为统一时间）
bool is_single_joint_; // 是否为单轴模式
};

118
src/robot_control/src/actions/action_manager.cpp
View File

@@ -1,8 +1,8 @@
/**
* @file action_manager.cpp
* @brief Action管理器实现
* @author Robot Control Team
* @date 2024
* @author Ray
* @date 2025
*/
#include "actions/action_manager.hpp"
@@ -92,7 +92,7 @@ void ActionManager::initialize()
std::bind(&ActionManager::handle_dual_arm_goal, this, _1, _2),
std::bind(&ActionManager::handle_dual_arm_cancel, this, _1),
std::bind(&ActionManager::handle_dual_arm_accepted, this, _1));
RCLCPP_INFO(node_->get_logger(), "DualArm action server is ready (using topic mode)");
RCLCPP_INFO(node_->get_logger(), "DualArm action server is ready");
}
// MoveHome Action 处理函数
@@ -534,6 +534,7 @@ void ActionManager::handle_dual_arm_accepted(const std::shared_ptr<GoalHandleDua
void ActionManager::dual_arm_execute(const std::shared_ptr<GoalHandleDualArm> goal_handle)
{
rclcpp::Rate loop_rate(10);
const auto goal = goal_handle->get_goal();
auto feedback = std::make_shared<DualArm::Feedback>();
auto result = std::make_shared<DualArm::Result>();
@@ -541,21 +542,37 @@ void ActionManager::dual_arm_execute(const std::shared_ptr<GoalHandleDualArm> go
result->success = false;
result->message = "DualArm action failed";
const MotionParameters& motionParams = robot_control_manager_.GetMotionParameters();
const size_t ARM_DOF = motionParams.arm_dof_; // 每条手臂的自由度数
try {
// 发送规划阶段反馈
feedback->status = 0; // STATUS_PLANNING = 0
feedback->progress = 0.0;
feedback->joints_left.clear();
feedback->joints_right.clear();
goal_handle->publish_feedback(feedback);
// 使用RobotControlManager的统一规划接口
for (const auto& arm_param : goal->arm_motion_params) {
// 检查参数有效性
if (arm_param.motion_type == arm_param.MOVEJ && arm_param.target_joints.empty()) {
RCLCPP_ERROR(node_->get_logger(), "MOVEJ requested but target_joints is empty for arm_id %d", arm_param.arm_id);
result->message = "MOVEJ target_joints empty";
goal_handle->abort(result);
dual_arm_executing_.store(false);
return;
}
if (arm_param.motion_type == arm_param.MOVEL) {
// MOVEL参数验证（target_pose在消息定义中已包含）
// 不需要额外验证
if (arm_param.motion_type == arm_param.MOVEJ) {
if (arm_param.target_joints.empty()) {
RCLCPP_ERROR(node_->get_logger(), "MOVEJ requested but target_joints is empty for arm_id %d", arm_param.arm_id);
result->message = "MOVEJ target_joints empty";
goal_handle->abort(result);
dual_arm_executing_.store(false);
return;
}
if (arm_param.target_joints.size() != ARM_DOF) {
RCLCPP_ERROR(node_->get_logger(), "MOVEJ requested but target_joints size (%zu) does not match arm_dof (%zu) for arm_id %d",
arm_param.target_joints.size(), ARM_DOF, arm_param.arm_id);
result->message = "MOVEJ target_joints size mismatch";
goal_handle->abort(result);
dual_arm_executing_.store(false);
return;
}
}
// 调用RobotControlManager的规划接口（统一管理）
@@ -595,6 +612,79 @@ void ActionManager::dual_arm_execute(const std::shared_ptr<GoalHandleDualArm> go
return;
}
// 执行阶段：循环发送反馈，直到轨迹执行完成
feedback->status = 1; // STATUS_EXECUTING = 1
while (dual_arm_executing_.load() && rclcpp::ok())
{
// 检查取消请求
if (goal_handle->is_canceling()) {
result->success = false;
result->message = "DualArm goal canceled";
goal_handle->canceled(result);
RCLCPP_INFO(node_->get_logger(), "DualArm goal canceled");
dual_arm_executing_.store(false);
return;
}
// 检查是否有轨迹在执行
bool left_arm_active = robot_control_manager_.HasActiveArmTrajectory(0);
bool right_arm_active = robot_control_manager_.HasActiveArmTrajectory(1);
// 计算进度（简单实现：如果还有轨迹在执行，进度为0.5，否则为1.0）
if (left_arm_active || right_arm_active) {
feedback->progress = 0.5;
} else {
feedback->progress = 1.0;
feedback->status = 2; // STATUS_DONE = 2
}
// 获取关节反馈并提取左右臂关节（单位：度）
auto joint_feedback = robot_control_manager_.GetJointFeedback();
feedback->joints_left.clear();
feedback->joints_right.clear();
// 提取左臂关节（dual_arm_joint_names_ 的前6个）
if (motionParams.dual_arm_joint_names_.size() >= ARM_DOF) {
for (size_t i = 0; i < ARM_DOF; ++i) {
const std::string& joint_name = motionParams.dual_arm_joint_names_[i];
auto it = motionParams.joint_name_to_index_.find(joint_name);
if (it != motionParams.joint_name_to_index_.end() && it->second < joint_feedback.data.size()) {
double angle_deg = joint_feedback.data[it->second];
double angle_rad = angle_deg * M_PI / 180.0; // 度转弧度
feedback->joints_left.push_back(angle_rad);
} else {
feedback->joints_left.push_back(0.0);
}
}
}
// 提取右臂关节（dual_arm_joint_names_ 的后6个）
if (motionParams.dual_arm_joint_names_.size() >= 2 * ARM_DOF) {
for (size_t i = ARM_DOF; i < 2 * ARM_DOF; ++i) {
const std::string& joint_name = motionParams.dual_arm_joint_names_[i];
auto it = motionParams.joint_name_to_index_.find(joint_name);
if (it != motionParams.joint_name_to_index_.end() && it->second < joint_feedback.data.size()) {
double angle_deg = joint_feedback.data[it->second];
double angle_rad = angle_deg * M_PI / 180.0; // 度转弧度
feedback->joints_right.push_back(angle_rad);
} else {
feedback->joints_right.push_back(0.0);
}
}
}
// 发布反馈
goal_handle->publish_feedback(feedback);
// 如果轨迹执行完成，退出循环
if (!left_arm_active && !right_arm_active) {
break;
}
loop_rate.sleep();
}
// 检查取消请求
if (goal_handle->is_canceling()) {
result->success = false;

68
src/robot_control/src/controllers/arm_control.cpp
View File

@@ -18,24 +18,17 @@ namespace robot_control
{
ArmControl::ArmControl(
size_t total_joints,
const std::vector<double>& lengthParameters,
const std::vector<double>& maxSpeed,
const std::vector<double>& maxAcc,
const std::vector<double>& minLimits,
const std::vector<double>& maxLimits,
const std::vector<double>& home_positions,
const std::vector<double>& zero_positions,
const std::vector<int>& joint_directions)
: ControlBase(total_joints, lengthParameters, maxSpeed, maxAcc,
minLimits, maxLimits, home_positions, zero_positions, joint_directions)
const std::vector<std::string>& joint_names,
const std::map<std::string, JointInfo>& joints_info_map,
const std::vector<double>& lengthParameters)
: ControlBase(joint_names, joints_info_map, lengthParameters)
, move_group_left_(nullptr)
, move_group_right_(nullptr)
, move_group_dual_(nullptr)
, node_(nullptr)
, current_arm_id_(0)
{
std::cout << "ArmControl Constructor: total_joints=" << total_joints << std::endl;
std::cout << "ArmControl Constructor: total_joints=" << joint_names.size() << std::endl;
// MoveIt配置从SRDF/URDF文件加载，但基础参数从MotionParameters获取
}
@@ -347,13 +340,20 @@ bool ArmControl::GetInterpolatedPoint(uint8_t arm_id, double dt, std::vector<dou
// 提取位置、速度、加速度信息
joint_positions = current_point.positions;
// 更新基类的速度和加速度数据成员
if (current_point.velocities.size() == joint_velocity_.size()) {
joint_velocity_ = current_point.velocities;
}
if (current_point.accelerations.size() == joint_acceleration_.size()) {
joint_acceleration_ = current_point.accelerations;
// 更新 joints_info_ 中的速度和加速度
for (size_t i = 0; i < joints_info_.size() && i < current_point.positions.size(); i++)
{
joints_info_[i].current_position = current_point.positions[i];
if (i < current_point.velocities.size())
{
joints_info_[i].current_velocity = current_point.velocities[i];
joints_info_[i].command_velocity = current_point.velocities[i];
}
if (i < current_point.accelerations.size())
{
joints_info_[i].current_acceleration = current_point.accelerations[i];
joints_info_[i].command_acceleration = current_point.accelerations[i];
}
}
// 更新索引和时间
@@ -430,8 +430,13 @@ bool ArmControl::MoveToTargetJoint(
if (GetInterpolatedPoint(current_arm_id_, dt, interpolated_joints))
{
joint_positions = interpolated_joints;
joint_commands_ = interpolated_joints;
// 注意：GetInterpolatedPoint 已经更新了 joint_velocity_ 和 joint_acceleration_
// 更新 joints_info_ 中的指令位置和状态
for (size_t i = 0; i < joints_info_.size() && i < interpolated_joints.size(); i++)
{
joints_info_[i].command_position = interpolated_joints[i];
joints_info_[i].current_position = interpolated_joints[i];
}
// 注意：GetInterpolatedPoint 会更新速度加速度，但需要通过其他方式获取并更新到 joints_info_
movingDurationTime_ += dt;
return false; // 还在运动
}
@@ -439,10 +444,16 @@ bool ArmControl::MoveToTargetJoint(
{
// 轨迹执行完成
joint_positions = target_joint;
joint_commands_ = target_joint;
// 轨迹完成时，将速度和加速度设置为0
std::fill(joint_velocity_.begin(), joint_velocity_.end(), 0.0);
std::fill(joint_acceleration_.begin(), joint_acceleration_.end(), 0.0);
// 更新 joints_info_ 中的指令位置和状态
for (size_t i = 0; i < joints_info_.size() && i < target_joint.size(); i++)
{
joints_info_[i].command_position = target_joint[i];
joints_info_[i].current_position = target_joint[i];
joints_info_[i].current_velocity = 0.0;
joints_info_[i].command_velocity = 0.0;
joints_info_[i].current_acceleration = 0.0;
joints_info_[i].command_acceleration = 0.0;
}
is_moving_ = false;
movingDurationTime_ = 0.0;
ClearTrajectory(current_arm_id_);
@@ -453,7 +464,12 @@ bool ArmControl::MoveToTargetJoint(
bool ArmControl::GoHome(std::vector<double>& joint_positions, double dt)
{
// 使用基类的home位置
return MoveToTargetJoint(joint_positions, joint_home_positions_, dt);
std::vector<double> home_positions(joints_info_.size());
for (size_t i = 0; i < joints_info_.size(); i++)
{
home_positions[i] = joints_info_[i].home_position;
}
return MoveToTargetJoint(joint_positions, home_positions, dt);
}
bool ArmControl::Stop(std::vector<double>& joint_positions, double dt)

183
src/robot_control/src/controllers/control_base.cpp
View File

@@ -1,4 +1,5 @@
#include "controllers/control_base.hpp"
#include "core/motion_parameters.hpp"
#include <stdexcept>
#include <vector>
#include <iostream>
@@ -7,36 +8,38 @@ namespace robot_control
{
ControlBase::ControlBase(
size_t total_joints,
const std::vector<double>& lengthParameters,
const std::vector<double>& maxSpeed,
const std::vector<double>& maxAcc,
const std::vector<double>& minLimits,
const std::vector<double>& maxLimits,
const std::vector<double>& home_positions,
const std::vector<double>& zero_positions,
const std::vector<int>& joint_directions
const std::vector<std::string>& joint_names,
const std::map<std::string, JointInfo>& joints_info_map,
const std::vector<double>& lengthParameters
):
total_joints_(total_joints),
joint_home_positions_(home_positions),
joint_min_limits_(minLimits),
joint_max_limits_(maxLimits),
total_joints_(joint_names.size()),
lengthParameters_(lengthParameters),
joint_directions_(joint_directions),
joint_zero_positions_(zero_positions),
is_target_set_(false),
planner_type_(TrajectoryPlannerType::TRAPEZOIDAL) // 默认使用梯形轨迹规划器
{
if (total_joints_ <= 0)
throw std::invalid_argument("Total joints must be positive");
// 初始化关节状态向量
joint_position_.resize(total_joints_, 0.0);
joint_velocity_.resize(total_joints_, 0.0);
joint_acceleration_.resize(total_joints_, 0.0);
joint_torque_.resize(total_joints_, 0.0);
joint_position_desired_.resize(total_joints_, 0.0);
joint_commands_.resize(total_joints_, 0.0);
// 从 joints_info_map 中提取信息，存储到 joints_info_ 中（状态和指令参数已在 JointInfo 构造函数中初始化为0）
joints_info_.resize(total_joints_);
std::vector<double> maxSpeed(total_joints_);
std::vector<double> maxAcc(total_joints_);
for (size_t i = 0; i < joint_names.size(); i++)
{
const std::string& name = joint_names[i];
auto it = joints_info_map.find(name);
if (it != joints_info_map.end())
{
joints_info_[i] = it->second;
maxSpeed[i] = joints_info_[i].max_velocity;
maxAcc[i] = joints_info_[i].max_acceleration;
}
else
{
throw std::invalid_argument("Joint name not found in joints_info_map");
}
}
// 初始化梯形轨迹规划器（使用多轴版本）
trapezoidalTrajectory_ = new TrapezoidalTrajectory(maxSpeed, maxAcc);
@@ -60,13 +63,15 @@ ControlBase::~ControlBase()
void ControlBase::SetHomePositions(const std::vector<double>& home_positions)
{
joint_home_positions_ = home_positions;
joint_position_desired_ = home_positions;
for (size_t i = 0; i < joints_info_.size() && i < home_positions.size(); i++)
{
joints_info_[i].home_position = home_positions[i];
}
}
bool ControlBase::MoveToTargetJoint(std::vector<double>& joint_positions, const std::vector<double>& target_joint, double dt)
{
if (joint_position_.size() != target_joint.size())
if (joints_info_.size() != target_joint.size())
{
throw std::invalid_argument("Joint position and target joint size do not match.");
}
@@ -76,15 +81,21 @@ bool ControlBase::MoveToTargetJoint(std::vector<double>& joint_positions, const
if (IsReached(target_joint))
{
joint_positions = target_joint;
// 更新 joints_info_ 中的位置
for (size_t i = 0; i < joints_info_.size() && i < target_joint.size(); i++)
{
joints_info_[i].current_position = target_joint[i];
joints_info_[i].command_position = target_joint[i];
}
return true;
}
// 从当前关节位置开始规划轨迹（根据规划器类型选择）
std::vector<double> current_pos = getCurrentPositions();
if (planner_type_ == TrajectoryPlannerType::TRAPEZOIDAL) {
trapezoidalTrajectory_->init(joint_position_, target_joint);
trapezoidalTrajectory_->init(current_pos, target_joint);
} else {
s_curveTrajectory_->init(joint_position_, target_joint);
s_curveTrajectory_->init(current_pos, target_joint);
}
movingDurationTime_ = 0.0;
is_moving_ = true;
@@ -103,40 +114,62 @@ bool ControlBase::MoveToTargetJoint(std::vector<double>& joint_positions, const
is_finished = s_curveTrajectory_->isFinished(movingDurationTime_);
}
// 获取速度和加速度（根据规划器类型）
std::vector<double> velocities, accelerations;
if (planner_type_ == TrajectoryPlannerType::TRAPEZOIDAL) {
velocities = trapezoidalTrajectory_->getVelocity();
accelerations = trapezoidalTrajectory_->getAcceleration();
} else {
velocities = s_curveTrajectory_->getVelocity();
accelerations = s_curveTrajectory_->getAcceleration();
}
// 检查是否到达目标位置
if (is_finished)
{
joint_positions = target_joint;
joint_commands_ = target_joint;
// 更新 joints_info_ 中的状态
for (size_t i = 0; i < joints_info_.size() && i < target_joint.size(); i++)
{
joints_info_[i].current_position = target_joint[i];
joints_info_[i].command_position = target_joint[i];
if (i < velocities.size()) joints_info_[i].current_velocity = velocities[i];
if (i < velocities.size()) joints_info_[i].command_velocity = velocities[i];
if (i < accelerations.size()) joints_info_[i].current_acceleration = accelerations[i];
if (i < accelerations.size()) joints_info_[i].command_acceleration = accelerations[i];
}
is_moving_ = false;
movingDurationTime_ = 0.0;
return true;
}
joint_commands_ = desired_pos;
joint_positions = desired_pos;
// 获取速度和加速度（根据规划器类型）
if (planner_type_ == TrajectoryPlannerType::TRAPEZOIDAL) {
joint_velocity_ = trapezoidalTrajectory_->getVelocity();
joint_acceleration_ = trapezoidalTrajectory_->getAcceleration();
} else {
joint_velocity_ = s_curveTrajectory_->getVelocity();
joint_acceleration_ = s_curveTrajectory_->getAcceleration();
// 更新 joints_info_ 中的状态
for (size_t i = 0; i < joints_info_.size() && i < desired_pos.size(); i++)
{
joints_info_[i].current_position = desired_pos[i];
joints_info_[i].command_position = desired_pos[i];
if (i < velocities.size()) joints_info_[i].current_velocity = velocities[i];
if (i < velocities.size()) joints_info_[i].command_velocity = velocities[i];
if (i < accelerations.size()) joints_info_[i].current_acceleration = accelerations[i];
if (i < accelerations.size()) joints_info_[i].command_acceleration = accelerations[i];
}
joint_positions = desired_pos;
return false;
}
bool ControlBase::GoHome(std::vector<double>& joint_positions, double dt)
{
if (!is_moving_)
// 构建 home 位置向量
std::vector<double> home_positions(joints_info_.size());
for (size_t i = 0; i < joints_info_.size(); i++)
{
joint_position_desired_ = joint_home_positions_;
home_positions[i] = joints_info_[i].home_position;
}
return MoveToTargetJoint(joint_positions, joint_position_desired_, dt);
// 无论是否在运动中，都移动到 home 位置
return MoveToTargetJoint(joint_positions, home_positions, dt);
}
@@ -159,14 +192,23 @@ bool ControlBase::Stop(std::vector<double>& joint_positions, double dt)
stopDurationTime_ += dt;
// 根据规划器类型更新急停轨迹
bool is_stop_finished;
std::vector<double> stop_positions;
if (planner_type_ == TrajectoryPlannerType::TRAPEZOIDAL) {
joint_positions = trapezoidalTrajectory_->updateStop(stopDurationTime_);
stop_positions = trapezoidalTrajectory_->updateStop(stopDurationTime_);
is_stop_finished = trapezoidalTrajectory_->isStopFinished(stopDurationTime_);
} else {
joint_positions = s_curveTrajectory_->updateStop(stopDurationTime_);
stop_positions = s_curveTrajectory_->updateStop(stopDurationTime_);
is_stop_finished = s_curveTrajectory_->isStopFinished(stopDurationTime_);
}
// 更新 joints_info_ 中的位置
for (size_t i = 0; i < joints_info_.size() && i < stop_positions.size(); i++)
{
joints_info_[i].current_position = stop_positions[i];
joints_info_[i].command_position = stop_positions[i];
}
joint_positions = stop_positions;
if (is_stop_finished)
{
is_moving_ = false;
@@ -185,21 +227,30 @@ void ControlBase::UpdateJointStates(
const std::vector<double>& joint_velocities,
const std::vector<double>& joint_torques)
{
// 2. 更新关节位置（同时同步关节角度，假设位置与角度单位一致）
joint_position_ = joint_positions;
// 更新 joints_info_ 中的当前状态
for (size_t i = 0; i < joints_info_.size() && i < joint_positions.size(); i++)
{
joints_info_[i].current_position = joint_positions[i];
if (i < joint_velocities.size())
{
joints_info_[i].current_velocity = joint_velocities[i];
}
if (i < joint_torques.size())
{
joints_info_[i].current_torque = joint_torques[i];
}
}
if( !is_joint_position_initialized_)
{
joint_commands_ = joint_position_;
// 初始化命令位置为当前位置
for (size_t i = 0; i < joints_info_.size() && i < joint_positions.size(); i++)
{
joints_info_[i].command_position = joint_positions[i];
}
is_joint_position_initialized_ = true;
std::cout << "joint commands initialized" << std::endl;
}
// 3. 更新关节速度
joint_velocity_ = joint_velocities;
// 4. 更新关节力矩
joint_torque_ = joint_torques;
}
bool ControlBase::IsMoving()
@@ -211,9 +262,9 @@ bool ControlBase::IsReached(const std::vector<double>& target_joint)
{
bool result = true;
for (size_t i = 0; i < target_joint.size(); i++)
for (size_t i = 0; i < target_joint.size() && i < joints_info_.size(); i++)
{
if (std::abs(joint_position_[i] - target_joint[i]) > POSITION_TOLERANCE)
if (std::abs(joints_info_[i].current_position - target_joint[i]) > POSITION_TOLERANCE)
{
result = false;
break;
@@ -230,19 +281,29 @@ void ControlBase::SetTrajectoryPlannerType(TrajectoryPlannerType planner_type)
bool ControlBase::checkJointLimits(std::vector<double>& joint_positions)
{
for (size_t i = 0; i < joint_positions.size() && i < joint_min_limits_.size() && i < joint_max_limits_.size(); i++)
for (size_t i = 0; i < joint_positions.size() && i < joints_info_.size(); i++)
{
if (joint_positions[i] < joint_min_limits_[i])
if (joint_positions[i] < joints_info_[i].min_limit)
{
joint_positions[i] = joint_min_limits_[i];
joint_positions[i] = joints_info_[i].min_limit;
}
else if (joint_positions[i] > joint_max_limits_[i])
else if (joint_positions[i] > joints_info_[i].max_limit)
{
joint_positions[i] = joint_max_limits_[i];
joint_positions[i] = joints_info_[i].max_limit;
}
}
return true;
}
std::vector<double> ControlBase::getCurrentPositions() const
{
std::vector<double> positions(joints_info_.size());
for (size_t i = 0; i < joints_info_.size(); i++)
{
positions[i] = joints_info_[i].current_position;
}
return positions;
}
} // namespace robot_control

57
src/robot_control/src/controllers/leg_control.cpp
View File

@@ -13,25 +13,11 @@ namespace robot_control
{
LegControl::LegControl(
size_t total_joints,
const std::vector<double>& lengthParameters,
const std::vector<double>& maxSpeed,
const std::vector<double>& maxAcc,
const std::vector<double>& minLimits,
const std::vector<double>& maxLimits,
const std::vector<double>& home_positions,
const std::vector<double>& zero_positions,
const std::vector<int>& joint_directions
) : ControlBase(
total_joints,
lengthParameters,
maxSpeed,
maxAcc,
minLimits,
maxLimits,
home_positions,
zero_positions,
joint_directions)
const std::vector<std::string>& joint_names,
const std::map<std::string, JointInfo>& joints_info_map,
const std::vector<double>& lengthParameters
) : ControlBase(joint_names, joints_info_map, lengthParameters)
, target_joint_positions_(joint_names.size(), 0.0)
{
std::cout << "LegControl Constructor" << std::endl;
}
@@ -56,7 +42,7 @@ bool LegControl::MoveUp(std::vector<double>& joint_positions, double dt)
}
}
bool result = MoveToTargetJoint(joint_positions, joint_position_desired_, dt);
bool result = MoveToTargetJoint(joint_positions, target_joint_positions_, dt);
if (result)
{
@@ -103,9 +89,10 @@ bool LegControl::SetMoveLegParametersInternal(double moveDistance)
std::vector<double> tempPosition;
tempPosition.resize(total_joints_, 0.0);
double backJoint1 = DEG2RAD(joint_position_[0] - joint_zero_positions_[0]);
double frountJoint1 = DEG2RAD(joint_position_[1] - joint_zero_positions_[1]);
double frountJoint2 = DEG2RAD(joint_position_[2] - joint_zero_positions_[2]);
// 使用默认零位置 0.0（已删除 joint_zero_positions_）
double backJoint1 = DEG2RAD(joints_info_[0].current_position);
double frountJoint1 = DEG2RAD(joints_info_[1].current_position);
double frountJoint2 = DEG2RAD(joints_info_[2].current_position);
double currentBackLegHeight = lengthParameters_[0] * abs(std::sin(backJoint1));
double currentFrountLegHeight = lengthParameters_[1] * abs(std::sin(frountJoint1)) + lengthParameters_[2] * abs(std::sin(frountJoint1 + frountJoint2));
@@ -151,12 +138,13 @@ bool LegControl::SetMoveLegParametersInternal(double moveDistance)
double frountFinalAngle2 = calculate_angle_from_links(lengthParameters_[1], lengthParameters_[2], tempFrountLegLength, true);
tempPosition[0] = joint_directions_[0] * backFinalAngle;
tempPosition[1] = joint_directions_[1] * (90.0 - frountFinalAngle1); //零点为水平方向
tempPosition[2] = joint_directions_[2] * (180.0 - frountFinalAngle2);
tempPosition[3] = joint_directions_[3] * (90.0 - frountFinalAngle1);
tempPosition[4] = joint_directions_[4] * (180.0 - frountFinalAngle2);
tempPosition[5] = joint_directions_[5] * backFinalAngle;
// 使用默认方向 1（正方向）
tempPosition[0] = backFinalAngle;
tempPosition[1] = (90.0 - frountFinalAngle1); //零点为水平方向
tempPosition[2] = (180.0 - frountFinalAngle2);
tempPosition[3] = (90.0 - frountFinalAngle1);
tempPosition[4] = (180.0 - frountFinalAngle2);
tempPosition[5] = backFinalAngle;
if (!checkJointLimits(tempPosition))
{
@@ -164,9 +152,10 @@ bool LegControl::SetMoveLegParametersInternal(double moveDistance)
return false;
}
for (size_t i = 0; i < joint_position_desired_.size(); i++)
// 使用默认零位置 0.0（已删除 joint_zero_positions_）
for (size_t i = 0; i < joints_info_.size() && i < tempPosition.size(); i++)
{
joint_position_desired_[i] = tempPosition[i] + joint_zero_positions_[i];
target_joint_positions_[i] = tempPosition[i];
}
std::cout << "Target relative joint Position: "
@@ -175,9 +164,9 @@ bool LegControl::SetMoveLegParametersInternal(double moveDistance)
<< tempPosition[4] << ", " << tempPosition[5] << std::endl;
std::cout << "Target abs Joint Position: "
<< joint_position_desired_[0] << ", " << joint_position_desired_[1] << ", "
<< joint_position_desired_[2] << ", " << joint_position_desired_[3] << ", "
<< joint_position_desired_[4] << ", " << joint_position_desired_[5] << std::endl;
<< target_joint_positions_[0] << ", " << target_joint_positions_[1] << ", "
<< target_joint_positions_[2] << ", " << target_joint_positions_[3] << ", "
<< target_joint_positions_[4] << ", " << target_joint_positions_[5] << std::endl;
is_target_set_ = true;

38
src/robot_control/src/controllers/waist_control.cpp
View File

@@ -1,4 +1,5 @@
#include "controllers/waist_control.hpp"
#include "core/motion_parameters.hpp"
#include <iostream>
#include <stdexcept>
#include <vector>
@@ -7,24 +8,11 @@ namespace robot_control
{
WaistControl::WaistControl(
size_t total_joints,
const std::vector<double>& lengthParameters,
const std::vector<double>& maxSpeed,
const std::vector<double>& maxAcc,
const std::vector<double>& minLimits,
const std::vector<double>& maxLimits,
const std::vector<double>& home_positions,
const std::vector<double>& zero_positions,
const std::vector<int>& joint_directions
): ControlBase(
total_joints,
lengthParameters,
maxSpeed, maxAcc,
minLimits,
maxLimits,
home_positions,
zero_positions,
joint_directions)
const std::vector<std::string>& joint_names,
const std::map<std::string, JointInfo>& joints_info_map,
const std::vector<double>& lengthParameters
): ControlBase(joint_names, joints_info_map, lengthParameters)
, target_joint_positions_(joint_names.size(), 0.0)
{
std::cout << "WaistControl Constructor" << std::endl;
}
@@ -50,11 +38,11 @@ bool WaistControl::MoveWaist(std::vector<double>& joint_positions, double dt)
if (!is_cmd_send_finished_)
{
is_cmd_send_finished_ = MoveToTargetJoint(joint_positions, joint_position_desired_, dt);
is_cmd_send_finished_ = MoveToTargetJoint(joint_positions, target_joint_positions_, dt);
}
else
{
if (IsReached(joint_position_desired_) || (time_out_count_ > TIME_OUT_COUNT))
if (IsReached(target_joint_positions_) || (time_out_count_ > TIME_OUT_COUNT))
{
std::cout << "Waist reached target position!" << std::endl;
is_target_set_ = false;
@@ -93,8 +81,8 @@ bool WaistControl::SetMoveWaistParametersInternal(double movepitch, double movey
std::vector<double> tempPosition;
tempPosition.resize(total_joints_, 0.0);
tempPosition[0] = joint_commands_[0] - joint_home_positions_[0] + movepitch;
tempPosition[1] = joint_commands_[1] - joint_home_positions_[1] + moveyaw;
tempPosition[0] = joints_info_[0].command_position - joints_info_[0].home_position + movepitch;
tempPosition[1] = joints_info_[1].command_position - joints_info_[1].home_position + moveyaw;
if (!checkJointLimits(tempPosition))
{
@@ -102,12 +90,12 @@ bool WaistControl::SetMoveWaistParametersInternal(double movepitch, double movey
return false;
}
for (size_t i = 0; i < joint_position_desired_.size(); i++)
for (size_t i = 0; i < joints_info_.size() && i < tempPosition.size(); i++)
{
joint_position_desired_[i] = tempPosition[i] + joint_home_positions_[i];
target_joint_positions_[i] = tempPosition[i] + joints_info_[i].home_position;
}
std::cout << "Waist Target Joint Position: " << joint_position_desired_[0] << ", " << joint_position_desired_[1] << std::endl;
std::cout << "Waist Target Joint Position: " << target_joint_positions_[0] << ", " << target_joint_positions_[1] << std::endl;
// std::cout << "Waist Target Joint Position: " << joint_position_desired_[0] << std::endl;
is_target_set_ = true;

123
src/robot_control/src/controllers/wheel_control.cpp
View File

@@ -1,4 +1,5 @@
#include "controllers/wheel_control.hpp"
#include "core/motion_parameters.hpp"
#include <iostream>
#include <vector>
@@ -6,26 +7,13 @@ namespace robot_control
{
WheelControl::WheelControl(
size_t total_joints,
const std::vector<double>& lengthParameters,
const std::vector<double>& maxSpeed,
const std::vector<double>& maxAcc,
const std::vector<double>& minLimits,
const std::vector<double>& maxLimits,
const std::vector<double>& home_positions,
const std::vector<double>& zero_positions,
const std::vector<int>& joint_directions
): ControlBase(
total_joints,
lengthParameters,
maxSpeed, maxAcc,
minLimits,
maxLimits,
home_positions,
zero_positions,
joint_directions)
const std::vector<std::string>& joint_names,
const std::map<std::string, JointInfo>& joints_info_map,
const std::vector<double>& lengthParameters
): ControlBase(joint_names, joints_info_map, lengthParameters)
, lastMoveDistance_(0.0)
, moveRatio_(0.0)
, target_joint_positions_(joint_names.size(), 0.0)
{
std::cout << "WheelControl Constructor" << std::endl;
}
@@ -39,8 +27,8 @@ int tempAdjustCount = 0;
bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, double moveWheelAngle)
{
double original1 = joint_position_desired_[0];
double original2 = joint_position_desired_[1];
double original1 = target_joint_positions_[0];
double original2 = target_joint_positions_[1];
std::vector<double> tempHomePositions;
std::vector<double> tempDesiredPositions;
@@ -50,8 +38,8 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub
for (size_t i = 0; i < total_joints_; i++)
{
tempHomePositions[i] = joint_home_positions_[i];
tempDesiredPositions[i] = joint_position_desired_[i];
tempHomePositions[i] = joints_info_[i].home_position;
tempDesiredPositions[i] = target_joint_positions_[i];
}
@@ -64,11 +52,11 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub
{
if (beta > 0)
{
tempHomePositions[0] = joint_home_positions_[0] - beta - 8;
tempHomePositions[0] = joints_info_[0].home_position - beta - 8;
}
else
{
tempHomePositions[0] = joint_home_positions_[0] - beta + 8;
tempHomePositions[0] = joints_info_[0].home_position - beta + 8;
}
}
@@ -77,31 +65,25 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub
tempAdjustCount ++;
if (beta > 0)
{
tempHomePositions[0] = joint_home_positions_[0] - beta * 2.0;
tempHomePositions[0] = joints_info_[0].home_position - beta * 2.0;
}
else
{
tempHomePositions[0] = joint_home_positions_[0] - beta;
tempHomePositions[0] = joints_info_[0].home_position - beta;
}
if (tempAdjustCount == 1)
{
tempHomePositions[0] - 40;
tempHomePositions[0] -= 40;
tempAdjustCount = 0;
}
}
for (size_t i = 0; i < joint_directions_.size(); i++)
// 使用默认方向 1（正方向）
for (size_t i = 0; i < total_joints_; i++)
{
if (joint_directions_[i] == 1)
{
tempDesiredPositions[i] = tempHomePositions[i] + theta;
}
else
{
tempDesiredPositions[i] = tempHomePositions[i] - theta; // * 0.98 * 1.02;
}
tempDesiredPositions[i] = tempHomePositions[i] + theta;
}
double a1 = abs(tempDesiredPositions[0] - original1);
@@ -120,30 +102,24 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub
if (moveRatio_ > 0.99 && moveRatio_ < 1.009)
{
for (size_t i = 0; i < joint_directions_.size(); i++)
// 使用默认方向 1（正方向）
for (size_t i = 0; i < total_joints_; i++)
{
if (joint_directions_[i] == 1)
{
joint_position_desired_[i] = joint_home_positions_[i] + theta;
}
else
{
joint_position_desired_[i] = joint_home_positions_[i] - theta; // * 0.98 * 1.02;
}
target_joint_positions_[i] = joints_info_[i].home_position + theta;
}
}
else
{
std::cout << "change distance " << std::endl;
for (size_t i = 0; i < joint_directions_.size(); i++)
for (size_t i = 0; i < total_joints_; i++)
{
joint_position_desired_[i] = tempDesiredPositions[i];
joint_home_positions_[i] = tempHomePositions[i];
target_joint_positions_[i] = tempDesiredPositions[i];
joints_info_[i].home_position = tempHomePositions[i];
}
}
std::cout << "motor 1 distance : " << joint_position_desired_[0] - original1 << std::endl;
std::cout << "motor 2 distance : " << joint_position_desired_[1] - original2 << std::endl;
std::cout << "motor 1 distance : " << target_joint_positions_[0] - original1 << std::endl;
std::cout << "motor 2 distance : " << target_joint_positions_[1] - original2 << std::endl;
std::cout << "Set move ratio : " << moveRatio_ << std::endl;
@@ -158,8 +134,8 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub
bool WheelControl::SetMoveWheelParametersInternalJog(double moveWheelDistance, double moveWheelAngle)
{
double original1 = joint_position_desired_[0];
double original2 = joint_position_desired_[1];
double original1 = target_joint_positions_[0];
double original2 = target_joint_positions_[1];
std::vector<double> tempHomePositions;
std::vector<double> tempDesiredPositions;
@@ -169,8 +145,8 @@ bool WheelControl::SetMoveWheelParametersInternalJog(double moveWheelDistance, d
for (size_t i = 0; i < total_joints_; i++)
{
tempHomePositions[i] = joint_home_positions_[i];
tempDesiredPositions[i] = joint_position_desired_[i];
tempHomePositions[i] = joints_info_[i].home_position;
tempDesiredPositions[i] = target_joint_positions_[i];
}
@@ -179,38 +155,27 @@ bool WheelControl::SetMoveWheelParametersInternalJog(double moveWheelDistance, d
double beta = moveWheelAngle / 360.0 * M_PI * 2 * lengthParameters_[1] / lengthParameters_[0] * 180.0 / M_PI;
for (size_t i = 0; i < joint_directions_.size(); i++)
{
tempHomePositions[0] = joint_home_positions_[0] + beta;
tempHomePositions[1] = joint_home_positions_[1] + beta;
}
// 使用默认方向 1（正方向）
tempHomePositions[0] = joints_info_[0].home_position + beta;
tempHomePositions[1] = joints_info_[1].home_position + beta;
for (size_t i = 0; i < joint_directions_.size(); i++)
for (size_t i = 0; i < total_joints_; i++)
{
if (joint_directions_[i] == 1)
{
tempHomePositions[i] += theta;
tempDesiredPositions[i] = tempHomePositions[i] ;
}
else
{
tempHomePositions[i] -= theta;
tempDesiredPositions[i] = tempHomePositions[i];
}
tempHomePositions[i] += theta;
tempDesiredPositions[i] = tempHomePositions[i];
}
moveRatio_ = 1;
for (size_t i = 0; i < joint_directions_.size(); i++)
for (size_t i = 0; i < total_joints_; i++)
{
joint_position_desired_[i] = tempDesiredPositions[i];
joint_home_positions_[i] = tempHomePositions[i];
target_joint_positions_[i] = tempDesiredPositions[i];
joints_info_[i].home_position = tempHomePositions[i];
}
std::cout << "motor 1 distance : " << joint_position_desired_[0] - original1 << std::endl;
std::cout << "motor 2 distance : " << joint_position_desired_[1] - original2 << std::endl;
std::cout << "motor 1 distance : " << target_joint_positions_[0] - original1 << std::endl;
std::cout << "motor 2 distance : " << target_joint_positions_[1] - original2 << std::endl;
std::cout << "Set move ratio : " << moveRatio_ << std::endl;
@@ -236,7 +201,11 @@ bool WheelControl::MoveWheel(std::vector<double>& joint_positions)
}
}
joint_positions = joint_position_desired_;
joint_positions.resize(joints_info_.size());
for (size_t i = 0; i < joints_info_.size(); i++)
{
joint_positions[i] = target_joint_positions_[i];
}
is_target_set_ = false;

179
src/robot_control/src/core/controller_factory.cpp
View File

@@ -1,8 +1,8 @@
/**
* @file controller_factory.cpp
* @brief 控制器工厂实现
* @author Robot Control Team
* @date 2024
* @author Ray
* @date 2025
*/
#include "core/controller_factory.hpp"
@@ -24,123 +24,74 @@ std::unique_ptr<ControlBase> ControllerFactory::create_controller(
{
switch (type)
{
case ControllerType::LEG_CONTROLLER:
case ControllerType::LEG_CONTROLLER: {
// 提取与腿部控制器相关的 joints_info
std::map<std::string, JointInfo> leg_joints_info;
for (const auto& joint_name : motion_params.leg_joint_names_)
{
auto it = motion_params.joints_info_map_.find(joint_name);
if (it != motion_params.joints_info_map_.end())
{
leg_joints_info[joint_name] = it->second;
}
}
return std::make_unique<LegControl>(
motion_params.leg_joint_indices_.size(),
motion_params.leg_length_,
motion_params.leg_max_velocity_,
motion_params.leg_max_acceleration_,
motion_params.leg_min_limit_,
motion_params.leg_max_limit_,
motion_params.leg_home_positions_,
motion_params.leg_zero_positions_,
motion_params.leg_joint_directions_);
case ControllerType::ARM_CONTROLLER: {
// ARM controller 继承自 ControlBase，需要从 MotionParameters 获取参数
// 每个臂6个关节，总共12个关节（左臂+右臂）
const size_t arm_total_joints = motion_params.left_arm_joint_indices_.size() +
motion_params.right_arm_joint_indices_.size();
// 合并左臂和右臂的参数
std::vector<double> arm_length_params;
arm_length_params.insert(arm_length_params.end(),
motion_params.left_arm_length_.begin(),
motion_params.left_arm_length_.end());
arm_length_params.insert(arm_length_params.end(),
motion_params.right_arm_length_.begin(),
motion_params.right_arm_length_.end());
std::vector<double> arm_max_speed;
arm_max_speed.insert(arm_max_speed.end(),
motion_params.left_arm_max_velocity_.begin(),
motion_params.left_arm_max_velocity_.end());
arm_max_speed.insert(arm_max_speed.end(),
motion_params.right_arm_max_velocity_.begin(),
motion_params.right_arm_max_velocity_.end());
std::vector<double> arm_max_acc;
arm_max_acc.insert(arm_max_acc.end(),
motion_params.left_arm_max_acceleration_.begin(),
motion_params.left_arm_max_acceleration_.end());
arm_max_acc.insert(arm_max_acc.end(),
motion_params.right_arm_max_acceleration_.begin(),
motion_params.right_arm_max_acceleration_.end());
// 限位（度转弧度）
std::vector<double> arm_min_limits, arm_max_limits;
for (size_t i = 0; i < motion_params.left_arm_min_limit_.size(); i++)
{
arm_min_limits.push_back(DEG2RAD(motion_params.left_arm_min_limit_[i]));
arm_max_limits.push_back(DEG2RAD(motion_params.left_arm_max_limit_[i]));
}
for (size_t i = 0; i < motion_params.right_arm_min_limit_.size(); i++)
{
arm_min_limits.push_back(DEG2RAD(motion_params.right_arm_min_limit_[i]));
arm_max_limits.push_back(DEG2RAD(motion_params.right_arm_max_limit_[i]));
}
// Home和Zero位置（已经是弧度）
std::vector<double> arm_home_positions;
arm_home_positions.insert(arm_home_positions.end(),
motion_params.left_arm_home_positions_.begin(),
motion_params.left_arm_home_positions_.end());
arm_home_positions.insert(arm_home_positions.end(),
motion_params.right_arm_home_positions_.begin(),
motion_params.right_arm_home_positions_.end());
std::vector<double> arm_zero_positions;
arm_zero_positions.insert(arm_zero_positions.end(),
motion_params.left_arm_zero_positions_.begin(),
motion_params.left_arm_zero_positions_.end());
arm_zero_positions.insert(arm_zero_positions.end(),
motion_params.right_arm_zero_positions_.begin(),
motion_params.right_arm_zero_positions_.end());
// 关节方向
std::vector<int> arm_joint_directions;
arm_joint_directions.insert(arm_joint_directions.end(),
motion_params.left_arm_joint_directions_.begin(),
motion_params.left_arm_joint_directions_.end());
arm_joint_directions.insert(arm_joint_directions.end(),
motion_params.right_arm_joint_directions_.begin(),
motion_params.right_arm_joint_directions_.end());
return std::make_unique<ArmControl>(
arm_total_joints,
arm_length_params,
arm_max_speed,
arm_max_acc,
arm_min_limits,
arm_max_limits,
arm_home_positions,
arm_zero_positions,
arm_joint_directions);
motion_params.leg_joint_names_,
leg_joints_info,
motion_params.leg_length_);
}
case ControllerType::WHEEL_CONTROLLER:
return std::make_unique<WheelControl>(
motion_params.wheel_joint_indices_.size(),
motion_params.wheel_length_,
motion_params.wheel_max_velocity_,
motion_params.wheel_max_acceleration_,
motion_params.wheel_min_limit_,
motion_params.wheel_max_limit_,
motion_params.wheel_home_positions_,
motion_params.wheel_zero_positions_,
motion_params.wheel_joint_directions_);
case ControllerType::ARM_CONTROLLER: {
// 提取与手臂控制器相关的 joints_info
std::map<std::string, JointInfo> arm_joints_info;
for (const auto& joint_name : motion_params.dual_arm_joint_names_)
{
auto it = motion_params.joints_info_map_.find(joint_name);
if (it != motion_params.joints_info_map_.end())
{
arm_joints_info[joint_name] = it->second;
}
}
return std::make_unique<ArmControl>(
motion_params.dual_arm_joint_names_,
arm_joints_info,
motion_params.dual_arm_length_);
}
case ControllerType::WAIST_CONTROLLER:
case ControllerType::WHEEL_CONTROLLER: {
// 提取与轮子控制器相关的 joints_info
std::map<std::string, JointInfo> wheel_joints_info;
for (const auto& joint_name : motion_params.wheel_joint_names_)
{
auto it = motion_params.joints_info_map_.find(joint_name);
if (it != motion_params.joints_info_map_.end())
{
wheel_joints_info[joint_name] = it->second;
}
}
return std::make_unique<WheelControl>(
motion_params.wheel_joint_names_,
wheel_joints_info,
motion_params.wheel_length_);
}
case ControllerType::WAIST_CONTROLLER: {
// 提取与腰部控制器相关的 joints_info
std::map<std::string, JointInfo> waist_joints_info;
for (const auto& joint_name : motion_params.waist_joint_names_)
{
auto it = motion_params.joints_info_map_.find(joint_name);
if (it != motion_params.joints_info_map_.end())
{
waist_joints_info[joint_name] = it->second;
}
}
return std::make_unique<WaistControl>(
motion_params.waist_joint_indices_.size(),
motion_params.waist_length_,
motion_params.waist_max_velocity_,
motion_params.waist_max_acceleration_,
motion_params.waist_min_limit_,
motion_params.waist_max_limit_,
motion_params.waist_home_positions_,
motion_params.waist_zero_positions_,
motion_params.waist_joint_directions_);
motion_params.waist_joint_names_,
waist_joints_info,
motion_params.waist_length_);
}
default:
return nullptr;

127
src/robot_control/src/core/motion_parameters.cpp Normal file
View File

@@ -0,0 +1,127 @@
/**
* @file motion_parameters.cpp
* @brief 运动参数类实现
* @author Ray
* @date 2025
*/
#include "core/motion_parameters.hpp"
#include <algorithm>
using namespace robot_control;
MotionParameters::MotionParameters()
{
// ==================== 结构参数 ====================
wheel_radius_ = 0.09; // 轮子半径（米）
wheel_separation_ = 0.55; // 轮距（米）
wheel_length_ = {
wheel_radius_,
wheel_separation_
};
// ==================== 尺寸参数 ====================
// 腿长参数（单位：米）
leg_length_ = {
0.66, // 后腿长
0.385, // 大腿长
0.362, // 小腿长
0.55, // 前腿髋部长度
0.4 // 前腿大腿长度
};
// ==================== 控制器启用标志 ====================
leg_controller_enabled_ = false;
wheel_controller_enabled_ = false;
waist_controller_enabled_ = false;
arm_controller_enabled_ = true;
waist_length_ = {0.1}; // 腰部长度参数（单位：米）
// 双臂长度参数（单位：米，每个臂6个关节对应的连杆长度）
// 注意：这些参数主要用于传统运动学计算，MoveIt会从URDF/SRDF加载实际参数
dual_arm_length_ = {0.1, 0.2, 0.15, 0.1, 0.05, 0.02, 0.1, 0.2, 0.15, 0.1, 0.05, 0.02}; // 双臂长度参数（单位：米，合并左右臂）
// ==================== 速度参数 ====================
// 轮子速度参数
max_linear_speed_x_ = 100; // 最大线速度 X（m/s）
max_linear_speed_z_ = 10; // 最大线速度 Z（m/s）
max_angular_speed_z_ = 50; // 最大角速度 Z（rad/s）
// ==================== 关节组名称列表（可配置，用于定义哪些关节属于哪个控制器）====================
wheel_joint_names_ = {"left_wheel_joint", "right_wheel_joint"}; // 轮子关节名称列表
waist_joint_names_ = {"waist_yaw_joint", "waist_pitch_joint"}; // 腰部关节名称列表
leg_joint_names_ = {"left_rear_hip_joint", "left_rear_knee_joint", "right_rear_hip_joint",
"right_rear_knee_joint", "left_front_hip_joint", "left_front_knee_joint"}; // 腿部关节名称列表
// 双臂关节名称列表（合并左右臂）
// dual_arm_joint_names_ = {"left_arm_joint_1", "left_arm_joint_2", "left_arm_joint_3",
// "left_arm_joint_4", "left_arm_joint_5", "left_arm_joint_6",
// "right_arm_joint_1", "right_arm_joint_2", "right_arm_joint_3",
// "right_arm_joint_4", "right_arm_joint_5", "right_arm_joint_6"};
dual_arm_joint_names_ = {"joint_1", "joint_2", " joint_3",
"joint_4", "joint_5", "joint_6",
"joint_7", "joint_8", "joint_9",
"joint_10", "joint_11", "joint_12"};
// ==================== 手臂参数 ====================
// 每条手臂的自由度数（关节数）
arm_dof_ = 6; // 每条手臂6个关节
// ==================== 初始化默认的 JointInfo 参数 ====================
// 轮子关节：无限位，默认速度5.0，加速度25.0，Home位置0.0
std::vector<double> wheel_home = {0.0, 0.0};
for (size_t i = 0; i < wheel_joint_names_.size(); i++)
{
double home_pos = (i < wheel_home.size()) ? wheel_home[i] : 0.0;
joints_info_map_[wheel_joint_names_[i]] = JointInfo(0.0, 0.0, LimitType::POSITION, 5.0, 25.0, home_pos);
}
// 腰部关节：默认限位±190度，速度50度/秒，加速度100度/秒²
std::vector<double> waist_limits = {40.0, 190.0};
std::vector<double> waist_home = {0.0, 0.0};
for (size_t i = 0; i < waist_joint_names_.size(); i++)
{
double max_limit = (i < waist_limits.size()) ? waist_limits[i] : 190.0;
double home_pos = (i < waist_home.size()) ? waist_home[i] : 0.0;
joints_info_map_[waist_joint_names_[i]] = JointInfo(max_limit, -max_limit, LimitType::POSITION, 50.0, 100.0, home_pos);
}
// 腿部关节：默认限位根据关节类型设置，速度50度/秒，加速度100度/秒²
std::vector<double> leg_max_limits = {40.0, 190.0, 0.0, 90.0, 60.0, 0.0};
std::vector<double> leg_min_limits = {-40.0, -190.0, -90.0, 0.0, 0.0, -90.0};
std::vector<double> leg_home = {217.52 - 65.0, 120.84 + 41.0, 108.7 + 40.63, 221.95 - 41.0, 234.14 - 29.504, 125.39 + 65.0};
for (size_t i = 0; i < leg_joint_names_.size(); i++)
{
double max_limit = (i < leg_max_limits.size()) ? leg_max_limits[i] : 180.0;
double min_limit = (i < leg_min_limits.size()) ? leg_min_limits[i] : -180.0;
double home_pos = (i < leg_home.size()) ? leg_home[i] : 0.0;
joints_info_map_[leg_joint_names_[i]] = JointInfo(max_limit, min_limit, LimitType::POSITION, 50.0, 100.0, home_pos);
}
// 双臂关节：默认限位±180度，速度30度/秒，加速度80度/秒²，Home位置0.0（弧度）
std::vector<double> dual_arm_home = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // 左臂Home位置
0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; // 右臂Home位置
for (size_t i = 0; i < dual_arm_joint_names_.size(); i++)
{
double home_pos = (i < dual_arm_home.size()) ? dual_arm_home[i] : 0.0;
joints_info_map_[dual_arm_joint_names_[i]] = JointInfo(180.0, -180.0, LimitType::POSITION, 30.0, 80.0, home_pos);
}
total_joints_ = wheel_joint_names_.size() + waist_joint_names_.size() + leg_joint_names_.size() + dual_arm_joint_names_.size();
}
void MotionParameters::InitializeJointNameMapping(const std::vector<std::string>& joint_names)
{
all_joint_names_ = joint_names;
joint_name_to_index_.clear();
// 建立名称到索引的映射
for (size_t i = 0; i < joint_names.size(); i++)
{
joint_name_to_index_[joint_names[i]] = i;
}
// 更新总关节数
total_joints_ = joint_names.size();
}

4
src/robot_control/src/core/remote_controller.cpp
View File

@@ -1,8 +1,8 @@
/**
* @file remote_controller.cpp
* @brief 遥控器控制器实现
* @author Robot Control Team
* @date 2024
* @author Ray
* @date 2025
*/
#include "core/remote_controller.hpp"

361
src/robot_control/src/core/robot_control_manager.cpp
View File

@@ -1,26 +1,26 @@
/**
* @file robot_control_manager.cpp
* @brief 机器人控制管理器实现
* @author Robot Control Team
* @date 2024
* @author Ray
* @date 2025
*/
#include "core/robot_control_manager.hpp"
#include <limits>
using namespace robot_control;
using namespace std;
using namespace std_msgs::msg;
RobotControlManager::RobotControlManager(const std::vector<std::string>& enabled_controllers)
RobotControlManager::RobotControlManager()
{
this->init(enabled_controllers);
this->init();
}
void RobotControlManager::init(const std::vector<std::string>& enabled_controllers)
void RobotControlManager::init()
{
jointInited_.resize(motionParams_.total_joints_, false);
// jointInited_.resize(3, false);
// Initialize the joint commands
for (size_t i = 0; i < motionParams_.total_joints_; i++)
@@ -35,50 +35,46 @@ void RobotControlManager::init(const std::vector<std::string>& enabled_controlle
is_wheel_jog_ = false;
// Initialize the wheel commands
for (size_t i = 0; i < motionParams_.wheel_joint_indices_.size(); i++)
for (size_t i = 0; i < motionParams_.wheel_joint_names_.size(); i++)
{
wheelCommands_.data.push_back(0.0);
}
// 初始化控制器启用标志
leg_controller_enabled_ = false;
wheel_controller_enabled_ = false;
waist_controller_enabled_ = false;
arm_controller_enabled_ = false;
leg_controller_enabled_ = motionParams_.leg_controller_enabled_;
wheel_controller_enabled_ = motionParams_.wheel_controller_enabled_;
waist_controller_enabled_ = motionParams_.waist_controller_enabled_;
arm_controller_enabled_ = motionParams_.arm_controller_enabled_;
// 动态加载控制器
if (ControllerFactory::is_controller_enabled(ControllerType::LEG_CONTROLLER, enabled_controllers))
if (leg_controller_enabled_)
{
leg_controller_ = std::unique_ptr<LegControl>(
static_cast<LegControl*>(ControllerFactory::create_controller(
ControllerType::LEG_CONTROLLER, motionParams_).release()));
leg_controller_enabled_ = true;
}
if (ControllerFactory::is_controller_enabled(ControllerType::WHEEL_CONTROLLER, enabled_controllers))
if (wheel_controller_enabled_)
{
wheel_controller_ = std::unique_ptr<WheelControl>(
static_cast<WheelControl*>(ControllerFactory::create_controller(
ControllerType::WHEEL_CONTROLLER, motionParams_).release()));
wheel_controller_enabled_ = true;
}
if (ControllerFactory::is_controller_enabled(ControllerType::WAIST_CONTROLLER, enabled_controllers))
if (waist_controller_enabled_)
{
waist_controller_ = std::unique_ptr<WaistControl>(
static_cast<WaistControl*>(ControllerFactory::create_controller(
ControllerType::WAIST_CONTROLLER, motionParams_).release()));
waist_controller_enabled_ = true;
}
if (ControllerFactory::is_controller_enabled(ControllerType::ARM_CONTROLLER, enabled_controllers))
if (arm_controller_enabled_)
{
auto arm_ctrl = ControllerFactory::create_controller(ControllerType::ARM_CONTROLLER, motionParams_);
if (arm_ctrl)
{
arm_controller_ = std::unique_ptr<ArmControl>(
static_cast<ArmControl*>(arm_ctrl.release()));
arm_controller_enabled_ = true;
}
}
@@ -88,6 +84,7 @@ void RobotControlManager::init(const std::vector<std::string>& enabled_controlle
isJointInitValueSet_ = false;
isGyroInited_ = false;
joint_name_mapping_initialized_ = false;
}
RobotControlManager::~RobotControlManager()
@@ -127,25 +124,42 @@ bool RobotControlManager::SetMoveWaistParameters(double movePitchAngle, double m
bool RobotControlManager::SetArmJointCommands(int8_t body_id, const std::vector<double>& joint_angles)
{
// Arm关节索引（左臂从9开始，右臂从15开始，每个臂6个关节）
const int LEFT_ARM_JOINT_START_INDEX = 9;
const int RIGHT_ARM_JOINT_START_INDEX = 15;
const int USED_ARM_DOF = 6;
if (joint_angles.size() < USED_ARM_DOF) {
// 从 dual_arm_joint_names_ 获取对应的关节名称（body_id 0=左臂前arm_dof_个，1=右臂后arm_dof_个）
if (!joint_name_mapping_initialized_ || motionParams_.dual_arm_joint_names_.empty()) {
std::cout << "Joint name mapping not initialized or dual_arm_joint_names_ is empty" << std::endl;
return false;
}
int start_index = (body_id == 0) ? LEFT_ARM_JOINT_START_INDEX : RIGHT_ARM_JOINT_START_INDEX;
const size_t ARM_DOF = motionParams_.arm_dof_; // 每条手臂的自由度数
const size_t start_idx = (body_id == 0) ? 0 : ARM_DOF; // 左臂从0开始，右臂从ARM_DOF开始
// 确保jointCommands_数组足够大
if (jointCommands_.data.size() < static_cast<size_t>(start_index + USED_ARM_DOF)) {
jointCommands_.data.resize(start_index + USED_ARM_DOF, 0.0);
if (joint_angles.size() < ARM_DOF) {
std::cout << "Joint angles size (" << joint_angles.size() << ") is less than required (" << ARM_DOF << ")" << std::endl;
return false;
}
// 设置关节命令（单位：度）
for (int i = 0; i < USED_ARM_DOF; ++i) {
jointCommands_.data[start_index + i] = joint_angles[i];
if (start_idx + ARM_DOF > motionParams_.dual_arm_joint_names_.size()) {
std::cout << "Arm joint names out of range for body_id: " << static_cast<int>(body_id) << std::endl;
return false;
}
// 确保jointCommands_数组足够大
if (jointCommands_.data.size() < motionParams_.total_joints_) {
jointCommands_.data.resize(motionParams_.total_joints_, 0.0);
}
// 设置关节命令（单位：度），通过关节名称获取索引
for (size_t i = 0; i < ARM_DOF; ++i) {
const std::string& joint_name = motionParams_.dual_arm_joint_names_[start_idx + i];
auto it = motionParams_.joint_name_to_index_.find(joint_name);
if (it != motionParams_.joint_name_to_index_.end()) {
size_t idx = it->second;
if (idx < jointCommands_.data.size()) {
jointCommands_.data[idx] = joint_angles[i];
}
} else {
std::cout << "Joint name not found in mapping: " << joint_name << std::endl;
}
}
return true;
@@ -298,7 +312,7 @@ Float64MultiArray RobotControlManager::GetJointFeedback()
Float64MultiArray RobotControlManager::GetWheelFeedback()
{
Float64MultiArray tempValues;
tempValues.data.resize(motionParams_.wheel_joint_indices_.size());
tempValues.data.resize(motionParams_.wheel_joint_names_.size());
for (size_t i = 0; i < wheelPositions_.size(); i++)
{
@@ -320,36 +334,49 @@ Float64MultiArray RobotControlManager::GetJointCommands()
Float64MultiArray tempValues;
tempValues.data.resize(motionParams_.total_joints_);
// Convert the joint commands to motor values and limit to encoder range
for (size_t i = 0; i < jointCommands_.data.size(); i++)
{
double angle = jointCommands_.data[i];
double normalizedAngle = angle ; // TODO : check angle fmod(angle, 360.0);
// 计算原始脉冲值
double pulseValue = normalizedAngle * motionParams_.degree_to_pulse_[i];
tempValues.data[i] = pulseValue;
tempValues.data[i] = jointCommands_.data[i];
}
return tempValues;
}
void RobotControlManager::CheckJointLimits(Float64MultiArray &tempJointValues)
{
// Check the joint limits
// Check the joint limits using joint names
for (size_t i = 0; i < tempJointValues.data.size(); i++)
{
if (tempJointValues.data[i] > motionParams_.joint_limits_[i].max)
double max_limit = std::numeric_limits<double>::max();
double min_limit = std::numeric_limits<double>::lowest();
// 使用名称映射访问关节信息（如果映射已初始化）
if (joint_name_mapping_initialized_ && i < motionParams_.all_joint_names_.size())
{
tempJointValues.data[i] = motionParams_.joint_limits_[i].max;
const std::string& joint_name = motionParams_.all_joint_names_[i];
if (motionParams_.joints_info_map_.find(joint_name) != motionParams_.joints_info_map_.end())
{
const auto& info = motionParams_.joints_info_map_[joint_name];
max_limit = info.max_limit;
min_limit = info.min_limit;
}
}
else
{
// 如果映射未初始化，使用默认值（向后兼容）
// 注意：这应该不会发生，因为映射应该在首次收到JointState时初始化
max_limit = std::numeric_limits<double>::max();
min_limit = std::numeric_limits<double>::lowest();
}
if (tempJointValues.data[i] < motionParams_.joint_limits_[i].min)
if (tempJointValues.data[i] > max_limit)
{
tempJointValues.data[i] = motionParams_.joint_limits_[i].min;
tempJointValues.data[i] = max_limit;
}
if (tempJointValues.data[i] < min_limit)
{
tempJointValues.data[i] = min_limit;
}
}
}
@@ -363,8 +390,6 @@ bool isAllTrueManual(const std::vector<bool>& vec) {
return true; // 所有元素都是 true
}
std::vector<double> RobotControlManager::GetImuDifference()
{
std::vector<double> result;
@@ -375,9 +400,6 @@ std::vector<double> RobotControlManager::GetImuDifference()
result[i] = gyroValues_[i] - gyroInitValues_[i];
}
//TODO: optimization
// gyroInitValues_[2] = gyroInitValues_[2] - 0.03;
std::cout << "get gyro init value : " << gyroInitValues_[2] << std::endl;
std::cout << "get gyro value : " << gyroValues_[2] << std::endl;
@@ -460,20 +482,15 @@ void RobotControlManager::QuaternionToRPYRad(double qw, double qx, double qy, do
void RobotControlManager::QuaternionToRPYDeg(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw)
{
QuaternionToRPYRad(qw, qx, qy, qz, roll, pitch, yaw);
roll = roll * 180.0 / M_PI;
pitch = pitch * 180.0 / M_PI;
yaw = yaw * 180.0 / M_PI;
}
void RobotControlManager::UpdateWheelStates(const MotorPos::SharedPtr msg)
void RobotControlManager::UpdateWheelStates(const MotorPos::SharedPtr msg)
{
motorPos_ = *msg;
wheelPositions_.resize(motionParams_.wheel_joint_indices_.size());
wheelVelocities_.resize(motionParams_.wheel_joint_indices_.size());
wheelEfforts_.resize(motionParams_.wheel_joint_indices_.size());
wheelPositions_.resize(motionParams_.wheel_joint_names_.size());
wheelVelocities_.resize(motionParams_.wheel_joint_names_.size());
if (motorPos_.motor_angle.size() != motionParams_.wheel_joint_indices_.size())
if (motorPos_.motor_angle.size() != motionParams_.wheel_joint_names_.size())
{
std::cout << "wheel states size is not equal to wheel numbles" << std::endl;
return;
@@ -483,7 +500,6 @@ void RobotControlManager::UpdateWheelStates(const MotorPos::SharedPtr msg)
{
wheelPositions_[i] = motorPos_.motor_angle[i];
wheelVelocities_[i] = 0.0;
wheelEfforts_[i] = 0.0;
}
if (wheel_controller_)
@@ -494,28 +510,52 @@ void RobotControlManager::UpdateWheelStates(const MotorPos::SharedPtr msg)
isWheelHomed_ = true;
}
wheel_controller_->UpdateJointStates(wheelPositions_, wheelVelocities_, wheelEfforts_);
wheel_controller_->UpdateJointStates(wheelPositions_, wheelVelocities_, std::vector<double>());
}
}
void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::SharedPtr msg)
{
jointStates_ = *msg;
// 首次收到 JointState 消息时，初始化关节名称映射
if (!joint_name_mapping_initialized_ && !jointStates_.name.empty())
{
motionParams_.InitializeJointNameMapping(jointStates_.name);
joint_name_mapping_initialized_ = true;
jointInited_.resize(motionParams_.total_joints_, false);
jointCommands_.data.resize(motionParams_.total_joints_, 0.0);
std::cout << "Joint name mapping initialized with " << motionParams_.total_joints_ << " joints" << std::endl;
}
jointPositions_.resize(motionParams_.total_joints_);
jointVelocities_.resize(motionParams_.total_joints_);
jointEfforts_.resize(motionParams_.total_joints_);
if (jointStates_.position.size() != motionParams_.total_joints_)
// 使用关节名称匹配来更新状态
for (size_t i = 0; i < jointStates_.position.size() && i < jointStates_.name.size(); i++)
{
std::cout << "Joint states size is not equal to total joints" << std::endl;
return;
}
for (size_t i = 0; i < jointStates_.position.size(); i++)
{
jointPositions_[i] = jointStates_.position[i] * motionParams_.pulse_to_degree_[i];
jointVelocities_[i] = jointStates_.velocity[i] * motionParams_.pulse_to_degree_[i];
jointEfforts_[i] = jointStates_.effort[i];
const std::string& joint_name = jointStates_.name[i];
// 如果名称映射已初始化，通过名称找到内部索引
if (joint_name_mapping_initialized_ && motionParams_.joint_name_to_index_.find(joint_name) != motionParams_.joint_name_to_index_.end())
{
size_t internal_index = motionParams_.joint_name_to_index_[joint_name];
if (internal_index < jointPositions_.size())
{
// pulse_to_degree 已删除，使用默认值 1.0
double pulse_to_degree = 1.0;
jointPositions_[internal_index] = jointStates_.position[i] * pulse_to_degree;
jointVelocities_[internal_index] = (i < jointStates_.velocity.size()) ? jointStates_.velocity[i] * pulse_to_degree : 0.0;
jointEfforts_[internal_index] = (i < jointStates_.effort.size()) ? jointStates_.effort[i] : 0.0;
}
}
else
{
std::cout << "joint name mapping not initialized" << std::endl;
return;
}
}
// TODO: This block can be deleted
@@ -549,50 +589,76 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
count++;
}
// Update the waist controller
size_t waistStartIndex = motionParams_.waist_joint_indices_[0];
size_t waistJointSize = motionParams_.waist_joint_indices_.size();
std::vector<double> waistPositions(
jointPositions_.begin() + waistStartIndex,
jointPositions_.begin() + waistStartIndex + waistJointSize
);
std::vector<double> waistVelocities(
jointVelocities_.begin() + waistStartIndex,
jointVelocities_.begin() + waistStartIndex + waistJointSize
);
std::vector<double> waistEfforts(
jointEfforts_.begin() + waistStartIndex,
jointEfforts_.begin() + waistStartIndex + waistJointSize
);
if (waist_controller_)
// Update the waist controller (使用名称映射后的索引)
if (waist_controller_ && joint_name_mapping_initialized_ && !motionParams_.waist_joint_names_.empty())
{
waist_controller_->UpdateJointStates(waistPositions, waistVelocities, waistEfforts);
std::vector<double> waistPositions;
std::vector<double> waistVelocities;
std::vector<double> waistEfforts;
std::vector<size_t> waist_indices = GetJointIndicesFromNames(motionParams_.waist_joint_names_);
for (size_t idx : waist_indices)
{
if (idx < jointPositions_.size())
{
waistPositions.push_back(jointPositions_[idx]);
waistVelocities.push_back((idx < jointVelocities_.size()) ? jointVelocities_[idx] : 0.0);
waistEfforts.push_back((idx < jointEfforts_.size()) ? jointEfforts_[idx] : 0.0);
}
}
if (!waistPositions.empty())
{
waist_controller_->UpdateJointStates(waistPositions, waistVelocities, waistEfforts);
}
}
// Update the leg controller
if (leg_controller_)
// Update the leg controller (使用名称映射后的索引)
if (leg_controller_ && joint_name_mapping_initialized_ && !motionParams_.leg_joint_names_.empty())
{
size_t legStartIndex = motionParams_.leg_joint_indices_[0];
size_t legJointSize = motionParams_.leg_joint_indices_.size();
std::vector<double> legPositions;
std::vector<double> legVelocities;
std::vector<double> legEfforts;
std::vector<size_t> leg_indices = GetJointIndicesFromNames(motionParams_.leg_joint_names_);
for (size_t idx : leg_indices)
{
if (idx < jointPositions_.size())
{
legPositions.push_back(jointPositions_[idx]);
legVelocities.push_back((idx < jointVelocities_.size()) ? jointVelocities_[idx] : 0.0);
legEfforts.push_back((idx < jointEfforts_.size()) ? jointEfforts_[idx] : 0.0);
}
}
if (!legPositions.empty())
{
leg_controller_->UpdateJointStates(legPositions, legVelocities, legEfforts);
}
}
std::vector<double> legPositions(
jointPositions_.begin() + legStartIndex,
jointPositions_.begin() + legStartIndex + legJointSize
);
std::vector<double> legVelocities(
jointVelocities_.begin() + legStartIndex,
jointVelocities_.begin() + legStartIndex + legJointSize
);
std::vector<double> legEfforts(
jointEfforts_.begin() + legStartIndex,
jointEfforts_.begin() + legStartIndex + legJointSize
);
leg_controller_->UpdateJointStates(legPositions, legVelocities, legEfforts);
// Update the arm controller (使用名称映射后的索引)
if (arm_controller_ && joint_name_mapping_initialized_ && !motionParams_.dual_arm_joint_names_.empty())
{
std::vector<double> armPositions;
std::vector<double> armVelocities;
std::vector<double> armEfforts;
std::vector<size_t> arm_indices = GetJointIndicesFromNames(motionParams_.dual_arm_joint_names_);
for (size_t idx : arm_indices)
{
if (idx < jointPositions_.size())
{
armPositions.push_back(jointPositions_[idx]);
armVelocities.push_back((idx < jointVelocities_.size()) ? jointVelocities_[idx] : 0.0);
armEfforts.push_back((idx < jointEfforts_.size()) ? jointEfforts_[idx] : 0.0);
}
}
if (!armPositions.empty())
{
arm_controller_->UpdateJointStates(armPositions, armVelocities, armEfforts);
}
}
}
@@ -693,13 +759,18 @@ bool RobotControlManager::MoveWheel()
return false;
}
tempWheelCmd_.resize(motionParams_.wheel_joint_indices_.size());
tempWheelCmd_.resize(motionParams_.wheel_joint_names_.size());
bool result = wheel_controller_->MoveWheel(tempWheelCmd_);
for (size_t i = 0; i < motionParams_.wheel_joint_indices_.size(); i++)
{
wheelCommands_.data[motionParams_.wheel_joint_indices_[i]] = tempWheelCmd_[i];
if (joint_name_mapping_initialized_) {
std::vector<size_t> wheel_indices = GetJointIndicesFromNames(motionParams_.wheel_joint_names_);
for (size_t i = 0; i < wheel_indices.size() && i < tempWheelCmd_.size(); i++)
{
if (wheel_indices[i] < wheelCommands_.data.size()) {
wheelCommands_.data[wheel_indices[i]] = tempWheelCmd_[i];
}
}
}
return result;
@@ -749,35 +820,69 @@ void RobotControlManager::JogAxis(size_t axis, int direction)
return;
}
jointCommands_.data[axis] += direction * motionParams_.jog_step_size_;
// jog_step_size_ 已删除，使用默认值 1.0 度
const double JOG_STEP_SIZE = 1.0;
jointCommands_.data[axis] += direction * JOG_STEP_SIZE;
}
void RobotControlManager::AssignTempValues()
{
tempWaistCmd_.resize(motionParams_.waist_joint_indices_.size());
if (!joint_name_mapping_initialized_) {
return;
}
std::vector<size_t> waist_indices = GetJointIndicesFromNames(motionParams_.waist_joint_names_);
tempWaistCmd_.resize(waist_indices.size());
for (size_t i = 0; i < motionParams_.waist_joint_indices_.size(); i++)
for (size_t i = 0; i < waist_indices.size(); i++)
{
tempWaistCmd_[i] = jointCommands_.data[motionParams_.waist_joint_indices_[i]];
if (waist_indices[i] < jointCommands_.data.size()) {
tempWaistCmd_[i] = jointCommands_.data[waist_indices[i]];
}
}
tempLegCmd_.resize(motionParams_.leg_joint_indices_.size());
std::vector<size_t> leg_indices = GetJointIndicesFromNames(motionParams_.leg_joint_names_);
tempLegCmd_.resize(leg_indices.size());
for (size_t i = 0; i < motionParams_.leg_joint_indices_.size(); i++)
for (size_t i = 0; i < leg_indices.size(); i++)
{
tempLegCmd_[i] = jointCommands_.data[motionParams_.leg_joint_indices_[i]];
if (leg_indices[i] < jointCommands_.data.size()) {
tempLegCmd_[i] = jointCommands_.data[leg_indices[i]];
}
}
}
std::vector<size_t> RobotControlManager::GetJointIndicesFromNames(const std::vector<std::string>& joint_names) const
{
std::vector<size_t> indices;
for (const auto& name : joint_names) {
auto it = motionParams_.joint_name_to_index_.find(name);
if (it != motionParams_.joint_name_to_index_.end()) {
indices.push_back(it->second);
}
}
return indices;
}
void RobotControlManager::UpdateJointCommands()
{
for (size_t i = 0; i < motionParams_.waist_joint_indices_.size(); i++)
if (!joint_name_mapping_initialized_) {
return;
}
std::vector<size_t> waist_indices = GetJointIndicesFromNames(motionParams_.waist_joint_names_);
for (size_t i = 0; i < waist_indices.size() && i < tempWaistCmd_.size(); i++)
{
jointCommands_.data[motionParams_.waist_joint_indices_[i]] = tempWaistCmd_[i];
if (waist_indices[i] < jointCommands_.data.size()) {
jointCommands_.data[waist_indices[i]] = tempWaistCmd_[i];
}
}
for (size_t i = 0; i < motionParams_.leg_joint_indices_.size(); i++)
std::vector<size_t> leg_indices = GetJointIndicesFromNames(motionParams_.leg_joint_names_);
for (size_t i = 0; i < leg_indices.size() && i < tempLegCmd_.size(); i++)
{
jointCommands_.data[motionParams_.leg_joint_indices_[i]] = tempLegCmd_[i];
if (leg_indices[i] < jointCommands_.data.size()) {
jointCommands_.data[leg_indices[i]] = tempLegCmd_[i];
}
}
}

103
src/robot_control/src/core/robot_control_node.cpp
View File

@@ -19,24 +19,25 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
// 初始化数据文件（设置路径，确保目录存在）
#if RECORD_FLAG
data_file_path_ = "/home/demo/ros2_joint_data.txt"; // 数据保存路径
fs::path file_path(data_file_path_);
fs::create_directories(file_path.parent_path()); // 自动创建父目录（如果不存在）
data_file_.open(data_file_path_, std::ios::trunc);
if (!data_file_.is_open()) {
} else {
// 写入表头（仅在文件为空时）
data_file_.seekp(0, std::ios::end); // 移动到文件末尾
}
data_file_path_ = "/home/demo/ros2_joint_data.txt"; // 数据保存路径
fs::path file_path(data_file_path_);
fs::create_directories(file_path.parent_path()); // 自动创建父目录（如果不存在）
data_file_.open(data_file_path_, std::ios::trunc);
if (!data_file_.is_open()) {
} else {
// 写入表头（仅在文件为空时）
data_file_.seekp(0, std::ios::end); // 移动到文件末尾
}
#endif
// 创建发布器和客户端（ActionManager 需要）
motorCmdPub_ = this->create_publisher<MotorCmd>("/motor_cmd", 10);
motorParamClient_ = this->create_client<MotorParam>("/motor_param");
jointTrajectoryPub_ = this->create_publisher<trajectory_msgs::msg::JointTrajectory>("/joint_trajectory", 10);
jointTrajectoryPub_ = this->create_publisher<trajectory_msgs::msg::JointTrajectory>("/trajectory_controller/joint_trajectory", 10);
gpioPub_ = this->create_publisher<control_msgs::msg::DynamicInterfaceGroupValues>("/gpio_command_controller/commands", 10);
imuMsgSub_ = this->create_subscription<ImuMsg>("/openzen/imu_msg", 10,std::bind(&RobotControlNode::ImuMsgCallback, this, std::placeholders::_1));
jointStatesSub_ = this->create_subscription<sensor_msgs::msg::JointState>("/ec_joint_states", 10,std::bind(&RobotControlNode::JointStatesCallback, this, std::placeholders::_1));
jointStatesSub_ = this->create_subscription<sensor_msgs::msg::JointState>("/joint_states", 10,std::bind(&RobotControlNode::JointStatesCallback, this, std::placeholders::_1));
wheelStatesSub_ = this->create_subscription<MotorPos>("/motor_pos", 10,std::bind(&RobotControlNode::WheelStatesCallback, this, std::placeholders::_1));
joyCommandSub_ = this->create_subscription<std_msgs::msg::String>("/gamepad_msg", 10,std::bind(&RobotControlNode::JoyCommandCallback, this, std::placeholders::_1));
controlTimer_ = this->create_wall_timer(std::chrono::milliseconds(CYCLE_TIME),std::bind(&RobotControlNode::ControlLoop, this));
@@ -129,7 +130,6 @@ void RobotControlNode::ControlLoop()
}
}
// 处理arm轨迹执行（在主循环中根据时间周期依次获取插补点位并下发）
if (action_manager_->is_dual_arm_executing())
{
if(robotControlManager_.MoveArm(dt_sec))
@@ -159,32 +159,35 @@ void RobotControlNode::Publish_joint_trajectory()
}
#if RECORD_FLAG
data_file_ << 0;
data_file_ << 0;
// 2. 保存整个数组数据到txt文件
if (data_file_.is_open()) {
for (const auto& val : cmd_msg.data) {
data_file_ << "," << val; // 用逗号分隔每个元素
// 2. 保存整个数组数据到txt文件
if (data_file_.is_open()) {
for (const auto& val : cmd_msg.data) {
data_file_ << "," << val; // 用逗号分隔每个元素
}
data_file_ << std::endl; // 换行
data_file_.flush(); // 强制刷新
}
data_file_ << std::endl; // 换行
data_file_.flush(); // 强制刷新
}
#endif
// 创建 JointTrajectory 消息
trajectory_msgs::msg::JointTrajectory trajectory_msg;
trajectory_msg.header.stamp = this->now();
trajectory_msg.header.frame_id = "base_link";
trajectory_msg.header.frame_id = "";
size_t total_joints = robotControlManager_.GetMotionParameters().total_joints_;
const MotionParameters& motion_params = robotControlManager_.GetMotionParameters();
size_t total_joints = motion_params.total_joints_;
// 设置关节名称（需要根据实际配置）
trajectory_msg.joint_names.resize(total_joints);
for (size_t i = 0; i < total_joints; ++i)
if (!motion_params.all_joint_names_.empty() && motion_params.all_joint_names_.size() == total_joints)
{
trajectory_msg.joint_names[i] = "joint_" + std::to_string(i);
trajectory_msg.joint_names = motion_params.all_joint_names_;
}
else
{
std::cout << "joint names not set!" << std::endl;
return;
}
// 创建轨迹点
@@ -244,3 +247,45 @@ void RobotControlNode::WheelStatesCallback(const MotorPos::SharedPtr cmd_msg)
robotControlManager_.UpdateWheelStates(cmd_msg);
}
bool RobotControlNode::activateController(const std::string& controller_name)
{
// TODO: Implement controller activation if needed
// This function is currently not used in the codebase
(void)controller_name; // Suppress unused parameter warning
return true;
}
void RobotControlNode::motor_reset_fault_all()
{
control_msgs::msg::DynamicInterfaceGroupValues faultMsg_;
const MotionParameters& motion_params = robotControlManager_.GetMotionParameters();
for(size_t i=0;i<motion_params.all_joint_names_.size();i++){
std::string tempInterfaceGroup = motion_params.all_joint_names_[i];
faultMsg_.interface_groups.push_back(tempInterfaceGroup);
control_msgs::msg::InterfaceValue tempValue;
//reset
tempValue.interface_names = {"fault"};
tempValue.values = {1};
faultMsg_.interface_values.push_back(tempValue);
}
gpioPub_->publish(faultMsg_);
usleep(20000);
}
void RobotControlNode::motor_enable_all(double enable)
{
control_msgs::msg::DynamicInterfaceGroupValues enableMsg_;
const MotionParameters& motion_params = robotControlManager_.GetMotionParameters();
for(size_t i=0;i<motion_params.all_joint_names_.size();i++){
std::string tempInterfaceGroup = motion_params.all_joint_names_[i];
enableMsg_.interface_groups.push_back(tempInterfaceGroup);
control_msgs::msg::InterfaceValue tempValue;
//enable
tempValue.interface_names = {"enable"};
tempValue.values = {enable};
enableMsg_.interface_values.push_back(tempValue);
}
gpioPub_->publish(enableMsg_);
usleep(20000);
}

4
src/robot_control/src/utils/right_arm_joint_test.cpp
View File

@@ -1,8 +1,8 @@
/**
* @file right_arm_joint_test.cpp
* @brief 右臂关节测试节点 - 独立可执行文件
* @author Robot Control Team
* @date 2024
* @author Ray
* @date 2025
*/
#include <rclcpp/rclcpp.hpp>

44
src/robot_control/src/utils/s_curve_trajectory.cpp
View File

@@ -372,29 +372,37 @@ void S_CurveTrajectory::calculateStopTrajectory()
stop_start_pos_ = current_pos_;
stop_start_vel_ = current_velocity_;
stop_start_acc_ = current_acceleration_;
stop_jerk_.resize(dof);
stop_decel_.resize(dof);
std::vector<double> stop_axis_time(dof, 0.0); // 各轴急停时间
// 使用最大Jerk进行急停
// 计算各轴所需急停时间和减速度
for (size_t i = 0; i < dof; ++i) {
double vel = std::fabs(stop_start_vel_[i]);
if (vel < 1e-6) {
stop_jerk_[i] = 0.0;
double vel = stop_start_vel_[i];
if (std::fabs(vel) < 1e-6) { // 静止轴无需减速
stop_decel_[i] = 0.0;
stop_axis_time[i] = 0.0;
continue;
}
// 使用最大Jerk，方向与速度相反
stop_jerk_[i] = -std::copysign(max_jerk_[i], stop_start_vel_[i]);
// 减速度方向与速度相反，大小不超过最大加速度
stop_decel_[i] = -std::copysign(max_acceleration_[i], vel);
// 急停时间 = 速度 / 减速度大小
stop_axis_time[i] = std::fabs(vel) / max_acceleration_[i];
}
// 简化：计算急停时间（使用最大减速度）
std::vector<double> stop_times(dof, 0.0);
// 统一急停时间（取最长时间）
stop_total_time_ = *std::max_element(stop_axis_time.begin(), stop_axis_time.end());
// 对于停止时间小于统一时间的轴，调整减速度使其在统一时间内停止
for (size_t i = 0; i < dof; ++i) {
if (std::fabs(stop_start_vel_[i]) < 1e-6) {
stop_times[i] = 0.0;
} else {
stop_times[i] = std::fabs(stop_start_vel_[i]) / max_acceleration_[i];
continue; // 静止轴保持减速度为0
}
if (stop_axis_time[i] > 0.0 && stop_axis_time[i] < stop_total_time_) {
// 调整减速度：vel0 + decel * stop_total_time_ = 0
stop_decel_[i] = -stop_start_vel_[i] / stop_total_time_;
}
}
stop_total_time_ = *std::max_element(stop_times.begin(), stop_times.end());
}
// 更新急停轨迹（简化版本）
@@ -404,22 +412,24 @@ std::vector<double> S_CurveTrajectory::updateStop(double dt)
throw std::runtime_error("Not in stop mode, call calculateStopTrajectory first");
size_t dof = stop_start_pos_.size();
double t = std::clamp(dt, 0.0, stop_total_time_);
double t = std::clamp(dt, 0.0, stop_total_time_); // 统一急停时间
for (size_t i = 0; i < dof; ++i) {
double vel0 = stop_start_vel_[i];
if (std::fabs(vel0) < 1e-6) {
double decel = stop_decel_[i];
if (std::fabs(vel0) < 1e-6) { // 静止轴保持不动
current_pos_[i] = stop_start_pos_[i];
current_velocity_[i] = 0.0;
current_acceleration_[i] = 0.0;
continue;
}
// 简化：使用匀减速
double decel = -std::copysign(max_acceleration_[i], vel0);
// 计算当前位置和速度（匀减速运动）
current_pos_[i] = stop_start_pos_[i] + vel0 * t + 0.5 * decel * t * t;
current_velocity_[i] = vel0 + decel * t;
// 避免微小速度（数值稳定性）
if (std::fabs(current_velocity_[i]) < 1e-6)
current_velocity_[i] = 0.0;
current_acceleration_[i] = decel;

14
src/robot_control/src/utils/trapezoidal_trajectory.cpp
View File

@@ -373,6 +373,20 @@ void TrapezoidalTrajectory::calculateStopTrajectory()
// 统一急停时间（取最长时间）
stop_total_time_ = *std::max_element(stop_axis_time.begin(), stop_axis_time.end());
// 对于停止时间小于统一时间的轴，调整减速度使其在统一时间内停止
for (size_t i = 0; i < dof; ++i)
{
if (std::fabs(stop_start_vel_[i]) < 1e-6)
{
continue; // 静止轴保持减速度为0
}
if (stop_axis_time[i] > 0.0 && stop_axis_time[i] < stop_total_time_)
{
// 调整减速度：vel0 + decel * stop_total_time_ = 0
stop_decel_[i] = -stop_start_vel_[i] / stop_total_time_;
}
}
}
// 更新急停轨迹