OpenLoong 项目“Hello World”，怎么让机器人挥挥手？

OpenLoong 项目"Hello World"

OpenLoong 作为一个全尺寸人形机器人的开源项目，其 "Hello World" 程序与传统的软件开发（如打印"Hello World"字符串）有所不同。

在机器人领域，"Hello World" 通常指的是让机器人完成一个最基础的、可视化的动作 ，以此验证硬件通路、控制系统和软件环境的正确性。对于 OpenLoong 来说，这个程序通常是**"让机器人站立（Stand Up）"或者"执行一个简单的关节动作"**。

以下是基于 OpenLoong 项目（通常基于 ROS 2 和全身动力学控制框架）编写"Hello World"程序的典型步骤和代码逻辑：

1. 核心逻辑：让机器人"站起来"

这是人形机器人的入门程序。你需要编写一个简单的 ROS 2 客户端节点，向机器人的控制话题发布目标关节角度或全身姿态指令。

Python 示例代码 (基于 ROS 2)

假设你已经启动了 OpenLoong 的仿真环境（如 Gazebo）或连接了真机，这段代码会让机器人尝试站立：

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
import time

class LoongHelloWorld(Node):
    def __init__(self):
        super().__init__('loong_hello_world')
        # 订阅关节状态（可选，用于调试）
        self.subscription = self.create_subscription(
            JointState,
            '/joint_states',
            self.listener_callback,
            10)
        
        # 发布轨迹目标到控制接口
        self.publisher_ = self.create_publisher(JointTrajectory, '/loong/trajectory_controller/joint_trajectory', 10)
        
        # 定义 OpenLoong 的关节名称 (示例，具体需参考 URDF 文件)
        self.joint_names = [
            'left_hip_joint', 'left_knee_joint', 'left_ankle_joint',
            'right_hip_joint', 'right_knee_joint', 'right_ankle_joint'
            # ... 其他关节
        ]

    def stand_up(self):
        """让机器人站立的"Hello World"动作"""
        msg = JointTrajectory()
        msg.joint_names = self.joint_names
        
        point = JointTrajectoryPoint()
        # 设置站立姿势的目标角度 (这里是示例数值，单位通常是弧度)
        # 具体数值需根据 OpenLoong 的零位和站立姿态调整
        point.positions = [0.0, -0.5, 0.3] * 2  # 左右对称
        point.time_from_start = rclpy.duration.Duration(seconds=2).to_msg() # 2秒内完成动作
        
        msg.points.append(point)
        
        self.publisher_.publish(msg)
        self.get_logger().info('OpenLoong 说: 你好，世界！ (站立动作已发布)')

    def listener_callback(self, data):
        # 这里可以打印当前关节状态
        pass

def main(args=None):
    rclpy.init(args=args)
    node = LoongHelloWorld()
    
    # 等待控制器准备就绪
    time.sleep(1)
    node.stand_up()
    
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

2. C++ 示例：发布简单的控制指令

如果你使用 C++（常用于高性能控制），代码结构类似，主要是在 main 函数中初始化节点并发布消息。

#include <rclcpp/rclcpp.hpp>
#include <trajectory_msgs/msg/joint_trajectory.hpp>

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("loong_hello_world");

  // 创建发布者
  auto publisher = node->create_publisher<trajectory_msgs::msg::JointTrajectory>("/loong/trajectory_controller/joint_trajectory", 10);

  // 构建站立姿态的消息
  auto msg = std::make_shared<trajectory_msgs::msg::JointTrajectory>();
  msg->joint_names = {"left_hip", "left_knee", /* ... */}; 

  trajectory_msgs::msg::JointTrajectoryPoint point;
  point.positions = {0.0, -0.5, /* ... */}; // 目标角度
  point.time_from_start = rclcpp::Duration(2s); // 持续时间

  msg->points.push_back(point);

  // 发布消息
  publisher->publish(*msg);
  RCLCPP_INFO(node->get_logger(), "OpenLoong 说: 你好，世界！");

  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

3. 编译与运行步骤

要让上述代码运行起来，你需要完成以下环境配置（这是 OpenLoong 开发的基础）：

1. 环境准备 ：

   • 安装 ROS 2 (如 Humble 或 Iron 版本)。
   • 获取 OpenLoong 的开源代码仓库（包含 URDF 模型和控制包）。

2. 创建工作空间 ：

   mkdir -p ~/openloong_ws/src
   cd ~/openloong_ws/src
   # 将你的 helloworld_node.py 放入一个包中，或者克隆相关功能包

3. 启动仿真（测试用） ：
   在运行代码前，必须先启动机器人的仿真模型，否则没有节点接收消息。

   # 在另一个终端启动 Gazebo 仿真
   ros2 launch loong_description loong_simulation.launch.py

4. 运行程序 ：

   ros2 run your_package_name helloworld_node.py

4. 预期结果

如果一切正常，你将在仿真界面（或真机上）看到 OpenLoong 机器人从躺平或初始姿态调整到站立姿态。同时在终端看到打印信息："OpenLoong 说: 你好，世界！"。

总结

对于 OpenLoong 项目，"Hello World" 不是打印文字，而是让机器人"动"起来。它验证了：

1. ROS 2 通信环境正常。
2. 控制器（如 joint_trajectory_controller）正常加载。
3. 你的代码能够正确发送指令并驱动硬件/仿真模型。

怎么让机器人挥挥手？

要让 OpenLoong 机器人挥手，本质上是控制其右臂（或左臂）的关节电机按照特定的轨迹运动 。这通常涉及动作序列的编程 或调用预设的动作库 。

以下是实现"挥手"动作的三种主要方法，从简单到复杂：

方法一：使用预设动作库 (最简单)

如果 OpenLoong 的底层控制库（如 loong_control 或 loong_gazebo 包）已经内置了常用动作，你可以直接调用。

Python 示例代码：

import rclpy
from rclpy.node import Node
# 假设有一个动作执行器接口
from loong_msgs.srv import ExecuteAction 

class WaveHello(Node):
    def __init__(self):
        super().__init__('wave_hello')
        # 创建客户端，连接到动作执行服务
        self.cli = self.create_client(ExecuteAction, '/loong/action_server')
        while not self.cli.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('动作服务器未启动，等待中...')

    def send_wave_command(self):
        request = ExecuteAction.Request()
        # 调用预定义的挥手动作
        request.action_name = "wave_hand" 
        # 发送异步请求
        self.future = self.cli.call_async(request)
        rclpy.spin_until_future_complete(self, self.future)
        self.get_logger().info("挥手动作已执行！")

def main():
    rclpy.init()
    node = WaveHello()
    node.send_wave_command()
    node.destroy_node()
    rclpy.shutdown()

方法二：手动规划关节轨迹 (核心原理)

如果系统没有预设挥手动作，你需要手动计算关节的角度变化。挥手通常涉及肩部关节（Pitch/Yaw）和肘部关节的协同运动。

Python 示例代码 (发布 JointTrajectory):

import rclpy
from rclpy.node import Node
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
import math

class LoongWaver(Node):
    def __init__(self):
        super().__init__('loong_waver')
        # 发布到右臂控制器
        self.pub = self.create_publisher(JointTrajectory, '/loong/right_arm_controller/joint_trajectory', 10)
        
        # 定义右臂关节名称 (具体名称需参考 URDF 文件，例如 loong.urdf)
        self.joint_names = [
            'right_shoulder_pitch', 'right_shoulder_yaw', 'right_elbow'
        ]

    def wave(self):
        msg = JointTrajectory()
        msg.joint_names = self.joint_names
        
        # 挥手通常是一个来回摆动的动作，这里模拟半个正弦波
        # 第一帧：挥手起始位置
        point1 = JointTrajectoryPoint()
        point1.positions = [0.0, 0.0, 0.5] # 肘部微弯
        point1.time_from_start.sec = 1
        msg.points.append(point1)

        # 第二帧：挥手最高点/外摆
        point2 = JointTrajectoryPoint()
        point2.positions = [0.4, -0.3, 0.5] # 肩部上抬并外摆
        point2.time_from_start.sec = 2
        msg.points.append(point2)

        # 第三帧：回到起始点 (完成一次挥手)
        point3 = JointTrajectoryPoint()
        point3.positions = [0.0, 0.0, 0.5]
        point3.time_from_start.sec = 3
        msg.points.append(point3)

        self.pub.publish(msg)
        self.get_logger().info("👋 挥手动作已发布！")

def main():
    rclpy.init()
    node = LoongWaver()
    # 等待1秒让订阅者发现话题
    node.create_timer(1.0, node.wave)
    rclpy.spin(node)
    rclpy.shutdown()

方法三：使用 MoveIt! 进行运动规划 (最灵活)

如果 OpenLoong 配置了 MoveIt!（一个强大的机器人运动规划框架），你可以通过设定**末端执行器（手）**的目标位姿来自动计算关节路径。

逻辑步骤：

1. 初始化 MoveIt! 的 MoveGroupCommander。
2. 设置目标姿态：让右手移动到身体右侧前方（挥手位置）。
3. 执行规划。

# 伪代码逻辑
from moveit_commander import MoveGroupCommander

right_arm = MoveGroupCommander("right_arm")
# 设置目标位置 (x, y, z) 和姿态
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = 0.3   # 向前0.3米
target_pose.position.y = -0.2  # 向右0.2米
target_pose.position.z = 0.1   # 向上0.1米

right_arm.set_pose_target(target_pose)
right_arm.go(wait=True) # 执行动作
right_arm.stop() # 停止

💡 关键注意事项

1. 关节名称必须准确 ：代码中的关节名称（如 right_shoulder_pitch）必须与 OpenLoong 的 URDF 模型文件完全一致。
2. 角度限制：确保你设定的角度在电机的物理限制范围内（例如，不要让肘部反转 180 度）。
3. 速度与加速度 ：在 JointTrajectoryPoint 中，最好设置 velocities 和 accelerations 字段，或者在 time_from_start 中留出足够的时间（如 1-2 秒），避免机器人动作过于生硬而摔倒。