6.2.9 关节电机控制
本示例展示了如何使用ROS2和Ruckig库来控制机器人的关节运动。
注意
注意⚠️ :运行本示例前需要在机器人运控计算单元(PC1)使用 aima em stop-app mc关闭原生运控模块，获取机器人控制权。注意确保机器人安全
！该示例代码直接对底层电机也就是HAL层进行控制，在运行程序前请检查代码中对关节的安全限位是否与所用机器人相符, 并确保安全！
机器人关节控制示例
这个示例展示了如何使用ROS2和Ruckig库来控制机器人的关节运动。示例实现了以下功能：
机器人关节模型定义
基于Ruckig的轨迹插补
多关节协同控制
实时位置、速度和加速度控制
依赖项
ROS2
Ruckig库
aimdk_msgs包
构建说明
将代码放入ROS2工作空间的src目录
在CMakeLists.txt中添加：
cmake
find_package(rclcpp REQUIRED)
find_package(aimdk_msgs REQUIRED)
find_package(ruckig REQUIRED)
add_executable(joint_control_example joint_control_example.cpp)
ament_target_dependencies(joint_control_example
rclcpp
aimdk_msgs
ruckig
)- 在package.xml中添加依赖：
xml
<depend>rclcpp</depend>
<depend>aimdk_msgs</depend>
<depend>ruckig</depend>示例功能说明
创建了四个控制器节点，分别控制：
腿部x2（12个关节）
腰部x1（3个关节）
手臂x2（14个关节）
头部x1（2个关节）
演示功能：
每10秒让指定关节在正负0.5弧度之间摆动
使用Ruckig库实现平滑的运动轨迹
实时发布关节控制命令
自定义使用
添加新的控制逻辑：
修改
SetTargetPosition函数添加新的控制回调函数
调整控制频率：
- 修改
control_timer_的周期（当前为2ms）
- 修改
cpp
#include <aimdk_msgs/msg/joint_command_array.hpp>
#include <aimdk_msgs/msg/joint_state_array.hpp>
#include <atomic>
#include <cstdlib>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <ruckig/ruckig.hpp>
#include <signal.h>
#include <string>
#include <unordered_map>
#include <vector>
/**
* @brief Global variables and signal handling
*/
// Global variables to control program state
std::atomic<bool> g_running(true);
std::atomic<bool> g_emergency_stop(false);
// Signal handler function
void signal_handler(int) {
g_running = false;
RCLCPP_INFO(rclcpp::get_logger("main"),
"Received termination signal, shutting down...");
}
/**
* @brief Robot model definition
*/
enum class JointArea {
HEAD, // Head joints
ARM, // Arm joints
WAIST, // Waist joints
LEG, // Leg joints
};
/**
* @brief Joint information structure
*/
struct JointInfo {
std::string name; // Joint name
double lower_limit; // Joint angle lower limit
double upper_limit; // Joint angle upper limit
double kp; // Position control gain
double kd; // Velocity control gain
};
/**
* @brief Robot model configuration
* Contains parameters for all joints, enabling or disabling specific joints as
* needed
*/
std::map<JointArea, std::vector<JointInfo>> robot_model = {
{JointArea::LEG,
{
// Left leg joint configuration
{"left_hip_pitch_joint", -2.704, 2.556, 40.0, 4.0},
{"left_hip_roll_joint", -0.235, 2.906, 40.0, 4.0},
{"left_hip_yaw_joint", -1.684, 3.430, 30.0, 3.0},
{"left_knee_joint", 0.0000, 2.4073, 80.0, 8.0},
{"left_ankle_pitch_joint", -0.803, 0.453, 40.0, 4.0},
{"left_ankle_roll_joint", -0.2625, 0.2625, 20.0, 2.0},
// Right leg joint configuration
{"right_hip_pitch_joint", -2.704, 2.556, 40.0, 4.0},
{"right_hip_roll_joint", -2.906, 0.235, 40.0, 4.0},
{"right_hip_yaw_joint", -3.430, 1.684, 30.0, 3.0},
{"right_knee_joint", 0.0000, 2.4073, 80.0, 8.0},
{"right_ankle_pitch_joint", -0.803, 0.453, 40.0, 4.0},
{"right_ankle_roll_joint", -0.2625, 0.2625, 20.0, 2.0},
}},
{JointArea::WAIST,
{
// Waist joint configuration
{"waist_yaw_joint", -3.43, 2.382, 20.0, 4.0},
{"waist_pitch_joint", -0.314, 0.314, 20.0, 4.0},
{"waist_roll_joint", -0.488, 0.488, 20.0, 4.0},
}},
{JointArea::ARM,
{
// Left arm joint configuration
{"left_shoulder_pitch_joint", -3.08, 2.04, 20.0, 2.0},
{"left_shoulder_roll_joint", -0.061, 2.993, 20.0, 2.0},
{"left_shoulder_yaw_joint", -2.556, 2.556, 20.0, 2.0},
{"left_elbow_joint", -2.3556, 0.0, 20.0, 2.0},
{"left_wrist_yaw_joint", -2.556, 2.556, 20.0, 2.0},
{"left_wrist_pitch_joint", -0.558, 0.558, 20.0, 2.0},
{"left_wrist_roll_joint", -1.571, 0.724, 20.0, 2.0},
// Right arm joint configuration
{"right_shoulder_pitch_joint", -3.08, 2.04, 20.0, 2.0},
{"right_shoulder_roll_joint", -2.993, 0.061, 20.0, 2.0},
{"right_shoulder_yaw_joint", -2.556, 2.556, 20.0, 2.0},
{"right_elbow_joint", -2.3556, 0.0000, 20.0, 2.0},
{"right_wrist_yaw_joint", -2.556, 2.556, 20.0, 2.0},
{"right_wrist_pitch_joint", -0.558, 0.558, 20.0, 2.0},
{"right_wrist_roll_joint", -0.724, 1.571, 20.0, 2.0},
}},
{JointArea::HEAD,
{
// Head joint configuration
{"head_yaw_joint", -0.366, 0.366, 20.0, 2.0},
{"head_pitch_joint", -0.3838, 0.3838, 20.0, 2.0},
}},
};
/**
* @brief Joint controller node class
* @tparam DOFs Degrees of freedom
* @tparam Area Joint area
*/
template <int DOFs, JointArea Area>
class JointControllerNode : public rclcpp::Node {
public:
/**
* @brief Constructor
* @param node_name Node name
* @param sub_topic Subscription topic name
* @param pub_topic Publication topic name
* @param qos QoS configuration
*/
JointControllerNode(std::string node_name, std::string sub_topic,
std::string pub_topic,
rclcpp::QoS qos = rclcpp::SensorDataQoS())
: Node(node_name), ruckig(0.002) {
joint_info_ = robot_model[Area];
if (joint_info_.size() != DOFs) {
RCLCPP_ERROR(this->get_logger(), "Joint count mismatch.");
exit(1);
}
// Set motion constraints for Ruckig trajectory planner
for (int i = 0; i < DOFs; ++i) {
input.max_velocity[i] = 1.0; // Max velocity limit
input.max_acceleration[i] = 1.0; // Max acceleration limit
input.max_jerk[i] = 25.0; // Max jerk (change of acceleration) limit
}
// Create joint state subscriber
sub_ = this->create_subscription<aimdk_msgs::msg::JointStateArray>(
sub_topic, qos,
std::bind(&JointControllerNode::JointStateCallback, this,
std::placeholders::_1));
// Create joint command publisher
pub_ = this->create_publisher<aimdk_msgs::msg::JointCommandArray>(pub_topic,
qos);
}
private:
// Ruckig trajectory planner variables
ruckig::Ruckig<DOFs> ruckig; // Trajectory planner instance
ruckig::InputParameter<DOFs> input; // Input parameters
ruckig::OutputParameter<DOFs> output; // Output parameters
bool ruckig_initialized_ = false; // Trajectory planner initialization flag
std::vector<JointInfo> joint_info_; // Joint information list
// ROS communication variables
rclcpp::Subscription<aimdk_msgs::msg::JointStateArray>::SharedPtr
sub_; // State subscriber
rclcpp::Publisher<aimdk_msgs::msg::JointCommandArray>::SharedPtr
pub_; // Command publisher
/**
* @brief Joint state callback function
* @param msg Joint state message
*/
void
JointStateCallback(const aimdk_msgs::msg::JointStateArray::SharedPtr msg) {
// Initialize trajectory planner on first state reception
if (!ruckig_initialized_) {
for (int i = 0; i < DOFs; ++i) {
input.current_position[i] = msg->joints[i].position;
input.current_velocity[i] = msg->joints[i].velocity;
input.current_acceleration[i] = 0.0;
}
ruckig_initialized_ = true;
RCLCPP_INFO(this->get_logger(),
"Ruckig trajectory planner initialization complete");
}
}
public:
/**
* @brief Set target joint position
* @param joint_name Joint name
* @param target_position Target position
* @return Whether the target position was successfully set
*/
bool SetTargetPosition(std::string joint_name, double target_position) {
if (!ruckig_initialized_) {
RCLCPP_WARN(this->get_logger(),
"Ruckig trajectory planner not initialized");
return false;
}
// Find target joint and set its position
int target_joint = -1;
for (int i = 0; i < DOFs; ++i) {
if (joint_info_[i].name == joint_name) {
// Check if target position is within limits
if (target_position < joint_info_[i].lower_limit ||
target_position > joint_info_[i].upper_limit) {
RCLCPP_ERROR(
this->get_logger(),
"Target position %.3f exceeds limit for joint %s [%.3f, %.3f]",
target_position, joint_name.c_str(), joint_info_[i].lower_limit,
joint_info_[i].upper_limit);
return false;
}
input.target_position[i] = target_position;
input.target_velocity[i] = 0.0;
input.target_acceleration[i] = 0.0;
target_joint = i;
} else {
input.target_position[i] = input.current_position[i];
input.target_velocity[i] = 0.0;
input.target_acceleration[i] = 0.0;
}
}
if (target_joint == -1) {
RCLCPP_ERROR(this->get_logger(), "Joint %s not found",
joint_name.c_str());
return false;
}
// Perform trajectory planning and send command using Ruckig
const double tolerance = 1e-6;
while (g_running && rclcpp::ok() && !g_emergency_stop) {
auto result = ruckig.update(input, output);
if (result != ruckig::Result::Working &&
result != ruckig::Result::Finished) {
RCLCPP_WARN(this->get_logger(), "Trajectory planning failed");
break;
}
// Update current state
for (int i = 0; i < DOFs; ++i) {
input.current_position[i] = output.new_position[i];
input.current_velocity[i] = output.new_velocity[i];
input.current_acceleration[i] = output.new_acceleration[i];
}
// Check if target position is reached
if (std::abs(output.new_position[target_joint] - target_position) <
tolerance) {
RCLCPP_INFO(this->get_logger(), "Joint %s reached target position",
joint_name.c_str());
break;
}
// Create and send joint command
aimdk_msgs::msg::JointCommandArray cmd;
cmd.joints.resize(DOFs);
for (int i = 0; i < DOFs; ++i) {
auto &joint = joint_info_[i];
cmd.joints[i].name = joint.name;
cmd.joints[i].position = output.new_position[i];
cmd.joints[i].velocity = output.new_velocity[i];
cmd.joints[i].stiffness = joint.kp;
cmd.joints[i].damping = joint.kd;
}
pub_->publish(cmd);
// Short delay to avoid excessive CPU usage
std::this_thread::sleep_for(std::chrono::milliseconds(2));
}
return true;
}
/**
* @brief Safely stop all joints
*/
void safe_stop() {
if (!ruckig_initialized_) {
RCLCPP_WARN(this->get_logger(), "Ruckig trajectory planner not "
"initialized, cannot perform safe stop");
return;
}
RCLCPP_INFO(this->get_logger(), "Performing safe stop...");
// Set all joint target positions to current positions
for (int i = 0; i < DOFs; ++i) {
input.target_position[i] = input.current_position[i];
input.target_velocity[i] = 0.0;
input.target_acceleration[i] = 0.0;
}
// Send final command to ensure joints stop
aimdk_msgs::msg::JointCommandArray cmd;
cmd.joints.resize(DOFs);
for (int i = 0; i < DOFs; ++i) {
auto &joint = joint_info_[i];
cmd.joints[i].name = joint.name;
cmd.joints[i].position = input.current_position[i];
cmd.joints[i].velocity = 0.0;
cmd.joints[i].stiffness = joint.kp;
cmd.joints[i].damping = joint.kd;
}
pub_->publish(cmd);
RCLCPP_INFO(this->get_logger(), "Safe stop complete");
}
/**
* @brief Emergency stop for all joints
*/
void emergency_stop() {
g_emergency_stop = true;
safe_stop();
RCLCPP_ERROR(this->get_logger(), "Emergency stop triggered");
}
};
/**
* @brief Main function
*/
int main(int argc, char *argv[]) {
rclcpp::init(argc, argv);
// Set up signal handling
signal(SIGINT, signal_handler);
signal(SIGTERM, signal_handler);
try {
// Create leg controller node
auto leg_node = std::make_shared<JointControllerNode<12, JointArea::LEG>>(
"leg_node", "/aima/hal/joint/leg/state", "/aima/hal/joint/leg/command");
// Create timer node
rclcpp::Node::SharedPtr timer_node =
rclcpp::Node::make_shared("timer_node");
double position = 0.8;
// Create timer callback function
auto timer = timer_node->create_wall_timer(std::chrono::seconds(3), [&]() {
if (!g_running || g_emergency_stop)
return; // If the program is shutting down or emergency stopped, do not
// execute new actions
position = -position;
position = 1.3 + position;
if (!leg_node->SetTargetPosition("left_knee_joint", position)) {
RCLCPP_ERROR(rclcpp::get_logger("main"),
"Failed to set target position");
}
});
// Create executor
rclcpp::executors::MultiThreadedExecutor executor;
executor.add_node(leg_node);
executor.add_node(timer_node);
// Main loop
while (g_running && rclcpp::ok() && !g_emergency_stop) {
executor.spin_once(std::chrono::milliseconds(100));
}
// Safely stop all joints
RCLCPP_INFO(rclcpp::get_logger("main"), "Safely stopping all joints...");
leg_node->safe_stop();
// Wait a short time to ensure command transmission is complete
std::this_thread::sleep_for(std::chrono::milliseconds(100));
// Clean up resources
RCLCPP_INFO(rclcpp::get_logger("main"), "Cleaning up resources...");
leg_node.reset();
timer_node.reset();
} catch (const std::exception &e) {
RCLCPP_ERROR(rclcpp::get_logger("main"), "Exception occurred: %s",
e.what());
g_emergency_stop = true;
} catch (...) {
RCLCPP_ERROR(rclcpp::get_logger("main"), "Unknown exception occurred");
g_emergency_stop = true;
}
RCLCPP_INFO(rclcpp::get_logger("main"), "Program exited safely");
rclcpp::shutdown();
return 0;
}