基于ROS2与MoveIt的宇树G1人形机器人传统舞蹈动作规划实战

最近在探索人形机器人的应用边界时，一个有趣的想法冒了出来：能否让宇树G1这样先进的机器人，穿上传统服饰，演绎一段富有文化韵味的舞蹈？这不仅是技术上的挑战，更是艺术与科技融合的一次尝试。本文将完整分享如何从零开始，为宇树G1机器人规划并实现一套传统服饰编舞表演的全过程。内容涵盖ROS2环境搭建、动作序列规划、仿真测试，以及如何将文化元素融入机器人动作设计。无论你是机器人开发的新手，还是希望拓展人形机器人应用场景的开发者，都能从中获得一套可直接复用的实战方案。

1. 项目背景与核心概念

1.1 宇树G1机器人简介

宇树G1是一款高性能、高自由度的人形机器人平台。它拥有强大的运动能力和开放的软件生态，是进行二次开发和算法验证的理想载体。G1机器人通常基于ROS2（机器人操作系统2）进行开发，这为开发者提供了标准化的通信、控制与仿真工具链。其核心优势在于灵活的关节控制、稳定的平衡算法以及丰富的传感器接口，使其能够完成复杂的全身协调运动。

1.2 传统服饰编舞表演的技术挑战

让机器人进行传统服饰表演，远不止是播放一段预设动作。它涉及到多个技术层面的整合：

1. 运动规划：传统舞蹈动作往往包含流畅的曲线、快速的转身和静态的造型。需要将舞蹈动作分解为机器人各关节（如肩、肘、髋、膝、踝）的角度序列。
2. 平衡控制：穿着模拟的宽大服饰（即便只是概念上的）会改变机器人的质心和动力学模型，在快速移动或单脚支撑时，平衡算法需要足够鲁棒。
3. 动作衔接与节奏：舞蹈讲究韵律。动作之间的过渡需要平滑自然，且需要严格跟随音乐的节拍，这对动作序列的时间戳控制提出了高要求。
4. 仿真验证：在实体机器人上直接测试高风险动作成本高昂且危险。必须先在Gazebo等物理仿真环境中进行充分的验证和调试。

1.3 核心工具链：ROS2与MoveIt

本项目将主要依赖ROS2和MoveIt运动规划框架。

• ROS2 (Robot Operating System 2)：提供节点通信、消息传递、服务调用等基础设施，是机器人软件的“中枢神经系统”。
• MoveIt：ROS生态中最重要的运动规划库。它集成了运动学求解、碰撞检测、轨迹规划等功能。我们可以利用MoveIt来规划和执行G1机器人的舞蹈动作序列。
• Gazebo：高保真物理仿真器。用于在虚拟世界中测试机器人的动作、平衡以及与环境（如地面、虚拟服饰）的交互，确保安全后再部署到真机。

2. 开发环境准备与项目搭建

2.1 基础环境配置

本文示例基于Ubuntu 22.04 LTS和ROS2 Humble Hawksbill版本。这是目前（截至知识截止日期）与宇树机器人SDK兼容性较好的稳定组合。

首先，安装ROS2 Humble：

# 设置语言环境 sudo apt update && sudo apt install locales sudo locale-gen en_US en_US.UTF-8 sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 export LANG=en_US.UTF-8 # 添加ROS2软件源 sudo apt install software-properties-common sudo add-apt-repository universe sudo apt update && sudo apt install curl -y sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null # 安装ROS2桌面版（包含Gazebo等工具） sudo apt update sudo apt install ros-humble-desktop

安装完成后，记得在~/.bashrc文件中添加ROS2环境变量：

echo “source /opt/ros/humble/setup.bash” >> ~/.bashrc source ~/.bashrc

2.2 宇树G1机器人模型与接口安装

宇树官方通常会在GitHub上提供机器人的URDF（统一机器人描述格式）模型和ROS2驱动包。这是进行仿真和控制的基石。

# 创建一个工作空间 mkdir -p ~/g1_dance_ws/src cd ~/g1_dance_ws/src # 假设宇树官方仓库为（请以实际官方仓库地址为准，此处为示例） git clone https://github.com/UnitreeRobotics/unitree_ros2.git # 或者克隆包含G1模型的ROS2包 # git clone https://github.com/UnitreeRobotics/unitree_g1_description.git cd ~/g1_dance_ws # 安装依赖并编译工作空间 rosdep install -i --from-path src --rosdistro humble -y colcon build --symlink-install source install/setup.bash

重要提示：请务必查阅宇树机器人官方文档或GitHub仓库，获取最新、最准确的模型和驱动安装方式。模型文件（urdf/xacro）描述了机器人的物理结构，是后续所有仿真和规划的基础。

2.3 安装MoveIt2

MoveIt2是ROS2下的运动规划框架。

sudo apt install ros-humble-moveit # 安装运动规划相关的其他工具 sudo apt install ros-humble-moveit-ros-visualization ros-humble-joint-state-publisher-gui

2.4 项目结构初始化

一个清晰的项目结构有助于管理代码、配置和资源。

~/g1_dance_ws/src/ ├── unitree_ros2/ # 宇树官方驱动和模型（从官方克隆） ├── g1_traditional_dance/ # 我们的编舞项目主包 │ ├── package.xml │ ├── CMakeLists.txt │ ├── config/ # 配置文件 │ │ ├── g1_dance.srdf # MoveIt语义机器人描述 │ │ ├── kinematics.yaml │ │ └── ompl_planning.yaml │ ├── launch/ # 启动文件 │ │ ├── dance_simulation.launch.py # 启动仿真和MoveIt │ │ └── dance_playback.launch.py # 播放动作序列 │ ├── meshes/ # （可选）存放传统服饰的3D模型文件 │ ├── scripts/ # Python脚本 │ │ ├── dance_planner.py # 动作规划器 │ │ └── pose_recorder.py # （可选）手动记录位姿工具 │ └── trajectories/ # 保存规划好的舞蹈动作序列文件 │ └── dance_sequence.yaml

使用以下命令创建我们的编舞功能包：

cd ~/g1_dance_ws/src ros2 pkg create --build-type ament_python g1_traditional_dance

3. 舞蹈动作规划核心原理

3.1 从舞蹈到关节角度：运动学求解

人体舞蹈动作可以看作是一系列关键“位姿”（Pose）的集合，每个位姿定义了身体各部位在空间中的位置和朝向。对于机器人，我们需要通过逆运动学（IK）求解，将末端执行器（如手、脚）的目标位姿，转换为机器人所有关节的角度。

例如，一个“拱手礼”动作，需要确定双手合抱时，左右手在空间中的目标位置。MoveIt的IK求解器会根据G1机器人的运动学链，计算出肩、肘、腕等关节需要转动的角度。

3.2 动作序列与轨迹生成

单个姿势是静态的。舞蹈是动态的，因此我们需要规划一条从姿势A到姿势B的运动轨迹。MoveIt的规划器（如OMPL）会考虑关节速度、加速度限制，以及避免自碰撞和环境碰撞，生成一条平滑、可行的关节空间轨迹。

轨迹由一系列路径点（Waypoint）组成，每个路径点包含所有关节在特定时间点的角度值。我们可以通过编程方式设置这些路径点，也可以使用MoveIt的“拖动示教”功能手动记录。

3.3 传统舞蹈动作的抽象与适配

传统舞蹈（如中国古典舞、日本舞踊）有其独特的动作语汇，如“圆场步”、“云手”、“探海”等。为机器人适配时，需注意：

• 简化与夸张：机器人关节自由度有限，无法完全复刻人类肌肉的细微控制。需要抓住动作的神韵，进行适当简化和几何化。
• 稳定性优先：一些高难度旋转或大跳动作，在真机上风险极高。在仿真中需反复测试平衡算法，或修改为更稳定的变体。
• 服饰的象征性处理：我们可能无法为G1穿上真实的丝绸水袖。但可以通过规划手臂的弧形运动轨迹，来模拟“甩袖”的视觉效果；通过控制上半身的缓慢旋转，来表现“裙摆摇曳”的意象。

4. 完整实战：编舞设计与仿真

4.1 启动G1机器人仿真环境

首先，我们需要在Gazebo中加载G1机器人模型，并启动MoveIt进行运动规划。

创建启动文件~/g1_dance_ws/src/g1_traditional_dance/launch/dance_simulation.launch.py：

# launch/dance_simulation.launch.py import os from launch import LaunchDescription from launch.actions import IncludeLaunchDescription, ExecuteProcess, RegisterEventHandler from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import PathJoinSubstitution, FindExecutable from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare from launch.event_handlers import OnProcessExit def generate_launch_description(): # 1. 启动Gazebo空世界 gazebo_world = IncludeLaunchDescription( PythonLaunchDescriptionSource([ PathJoinSubstitution([ FindPackageShare('gazebo_ros'), 'launch', 'gazebo.launch.py' ]) ]), launch_arguments={'world': 'empty'}.items() ) # 2. 将G1机器人URDF模型生成节点 # 假设宇树模型包名为 ‘unitree_g1_description’ pkg_path = FindPackageShare('unitree_g1_description') urdf_file = os.path.join(pkg_path, 'urdf', 'g1.urdf.xacro') # 具体文件名需确认 with open(urdf_file, 'r') as infp: robot_desc = infp.read() robot_state_publisher = Node( package='robot_state_publisher', executable='robot_state_publisher', name='robot_state_publisher', output='screen', parameters=[{'robot_description': robot_desc}] ) # 3. 在Gazebo中生成机器人模型 spawn_entity = Node( package='gazebo_ros', executable='spawn_entity.py', arguments=['-entity', 'g1', '-topic', 'robot_description'], output='screen' ) # 4. 启动MoveIt2 moveit_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource([ PathJoinSubstitution([ FindPackageShare('g1_traditional_dance'), # 我们的包 'launch', 'moveit.launch.py' # 需要另外创建MoveIt配置的启动文件 ]) ]) ) return LaunchDescription([ gazebo_world, robot_state_publisher, RegisterEventHandler( event_handler=OnProcessExit( target_action=robot_state_publisher, on_exit=[spawn_entity] ) ), moveit_launch, ])

注意：moveit.launch.py和g1_dance.srdf等MoveIt配置文件需要通过MoveIt Setup Assistant工具生成，这是一个图形化工具，用于配置机器人的运动学组、规划组、末端效应器等。由于篇幅限制，此处不展开，读者可查阅MoveIt官方教程完成配置。

4.2 编写舞蹈动作规划脚本

接下来，我们编写核心的Python脚本，用于定义舞蹈动作并调用MoveIt执行。

创建脚本~/g1_dance_ws/src/g1_traditional_dance/scripts/dance_planner.py：

#!/usr/bin/env python3 # scripts/dance_planner.py import rclpy from rclpy.node import Node from geometry_msgs.msg import Pose, Point, Quaternion from moveit_msgs.msg import CollisionObject from shape_msgs.msg import SolidPrimitive from moveit_msgs.srv import GetPositionIK from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint import yaml import math import time class DancePlanner(Node): def __init__(self): super().__init__('dance_planner') self.get_logger().info('舞蹈动作规划器启动...') # 定义G1机器人关节名称（示例，需根据实际URDF调整） self.joint_names = [ 'left_hip_yaw', 'left_hip_roll', 'left_hip_pitch', 'left_knee', 'left_ankle_pitch', 'left_ankle_roll', 'right_hip_yaw', 'right_hip_roll', 'right_hip_pitch', 'right_knee', 'right_ankle_pitch', 'right_ankle_roll', 'torso_yaw', 'left_shoulder_pitch', 'left_shoulder_roll', 'left_elbow', 'right_shoulder_pitch', 'right_shoulder_roll', 'right_elbow', 'head_pan', 'head_tilt' ] # 定义几个关键舞蹈姿势的关节角度（单位：弧度） # 姿势1：起始立正姿势 self.pose_home = [0.0] * len(self.joint_names) # 姿势2：模拟“起手式”，双臂微展，身体略侧 self.pose_begin = self.pose_home.copy() self.pose_begin[self.joint_names.index('left_shoulder_pitch')] = -0.3 self.pose_begin[self.joint_names.index('right_shoulder_pitch')] = -0.3 self.pose_begin[self.joint_names.index('torso_yaw')] = 0.1 # 姿势3：模拟“云手”动作的中间态，左手高，右手低，身体扭转 self.pose_cloud_hands = self.pose_home.copy() self.pose_cloud_hands[self.joint_names.index('left_shoulder_pitch')] = -1.5 self.pose_cloud_hands[self.joint_names.index('left_shoulder_roll')] = 0.5 self.pose_cloud_hands[self.joint_names.index('right_shoulder_pitch')] = -0.5 self.pose_cloud_hands[self.joint_names.index('right_shoulder_roll')] = -0.3 self.pose_cloud_hands[self.joint_names.index('torso_yaw')] = 0.4 # 姿势4：模拟“行礼”，身体前倾，双手作揖 self.pose_bow = self.pose_home.copy() self.pose_bow[self.joint_names.index('left_hip_pitch')] = 0.4 self.pose_bow[self.joint_names.index('right_hip_pitch')] = 0.4 self.pose_bow[self.joint_names.index('left_shoulder_pitch')] = -1.2 self.pose_bow[self.joint_names.index('right_shoulder_pitch')] = -1.2 self.pose_bow[self.joint_names.index('left_elbow')] = 1.57 self.pose_bow[self.joint_names.index('right_elbow')] = 1.57 def create_trajectory_point(self, joint_positions, time_from_start_sec): """创建一个轨迹点""" point = JointTrajectoryPoint() point.positions = joint_positions point.time_from_start = rclpy.duration.Duration(seconds=time_from_start_sec).to_msg() # 可以在这里设置速度和加速度 # point.velocities = [0.0] * len(joint_positions) # point.accelerations = [0.0] * len(joint_positions) return point def plan_dance_sequence(self): """规划一段简单的舞蹈序列""" trajectory = JointTrajectory() trajectory.joint_names = self.joint_names # 定义序列：起始 -> 起手式 -> 云手 -> 行礼 -> 回到起始 # 每个动作持续2秒 trajectory.points.append(self.create_trajectory_point(self.pose_home, 0.0)) trajectory.points.append(self.create_trajectory_point(self.pose_begin, 2.0)) trajectory.points.append(self.create_trajectory_point(self.pose_cloud_hands, 4.0)) trajectory.points.append(self.create_trajectory_point(self.pose_bow, 6.0)) trajectory.points.append(self.create_trajectory_point(self.pose_home, 8.0)) self.get_logger().info('舞蹈轨迹规划完成，共%d个路径点。' % len(trajectory.points)) return trajectory def save_trajectory_to_yaml(self, trajectory, filename): """将轨迹保存为YAML文件，方便后续调用""" data = { 'joint_names': trajectory.joint_names, 'points': [] } for point in trajectory.points: point_data = { 'positions': point.positions, 'time_from_start': point.time_from_start.sec + point.time_from_start.nanosec * 1e-9 } data['points'].append(point_data) filepath = f'~/g1_dance_ws/src/g1_traditional_dance/trajectories/{filename}' with open(filepath, 'w') as f: yaml.dump(data, f, default_flow_style=False) self.get_logger().info(f'轨迹已保存至 {filepath}') def main(args=None): rclpy.init(args=args) dancer = DancePlanner() # 规划舞蹈序列 dance_trajectory = dancer.plan_dance_sequence() # 保存到文件 dancer.save_trajectory_to_yaml(dance_trajectory, 'simple_dance.yaml') # 在实际项目中，这里会通过Action或Service将轨迹发送给机器人控制器执行 # 例如：使用 moveit_msgs/action/ExecuteTrajectory dancer.get_logger().info('舞蹈规划结束，请查看保存的YAML文件。') dancer.destroy_node() rclpy.shutdown() if __name__ == '__main__': main()

4.3 在仿真中执行舞蹈

规划好轨迹后，我们需要一个节点来读取YAML文件并控制仿真环境中的机器人运动。这里创建一个简单的播放器节点。

创建脚本~/g1_dance_ws/src/g1_traditional_dance/scripts/trajectory_player.py：

#!/usr/bin/env python3 # scripts/trajectory_player.py import rclpy from rclpy.node import Node from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint from control_msgs.action import FollowJointTrajectory from rclpy.action import ActionClient import yaml import time class TrajectoryPlayer(Node): def __init__(self): super().__init__('trajectory_player') # 创建Action客户端，连接到机器人的轨迹控制器 # 控制器名称需根据实际配置调整，例如 ‘/joint_trajectory_controller/follow_joint_trajectory’ self._action_client = ActionClient( self, FollowJointTrajectory, '/joint_trajectory_controller/follow_joint_trajectory' ) self.get_logger().info('等待轨迹动作服务器...') self._action_client.wait_for_server() def load_trajectory_from_yaml(self, filepath): """从YAML文件加载轨迹""" with open(filepath, 'r') as f: data = yaml.safe_load(f) trajectory = JointTrajectory() trajectory.joint_names = data['joint_names'] for p in data['points']: point = JointTrajectoryPoint() point.positions = p['positions'] point.time_from_start = rclpy.duration.Duration(seconds=p['time_from_start']).to_msg() trajectory.points.append(point) return trajectory def send_trajectory_goal(self, trajectory): """发送轨迹目标""" goal_msg = FollowJointTrajectory.Goal() goal_msg.trajectory = trajectory self.get_logger().info('发送舞蹈轨迹目标...') self._action_client.send_goal_async(goal_msg) self.get_logger().info('目标已发送，机器人开始舞蹈。') def main(args=None): rclpy.init(args=args) player = TrajectoryPlayer() # 加载之前保存的舞蹈序列 trajectory = player.load_trajectory_from_yaml( '~/g1_dance_ws/src/g1_traditional_dance/trajectories/simple_dance.yaml' ) # 发送给机器人执行 player.send_trajectory_goal(trajectory) # 保持节点运行，直到动作完成（简单示例，实际应用需更完善的反馈处理） try: rclpy.spin(player) except KeyboardInterrupt: pass finally: player.destroy_node() rclpy.shutdown() if __name__ == '__main__': main()

4.4 运行与验证

1. 编译工作空间：

   cd ~/g1_dance_ws colcon build --packages-select g1_traditional_dance source install/setup.bash

2. 启动仿真环境：

   ros2 launch g1_traditional_dance dance_simulation.launch.py

   如果一切正常，Gazebo会打开并显示G1机器人模型，RViz（通过MoveIt启动）也会打开，显示机器人的规划场景。

3. 生成舞蹈轨迹（在新终端）：

   source ~/g1_dance_ws/install/setup.bash ros2 run g1_traditional_dance dance_planner

   这会在trajectories/目录下生成simple_dance.yaml文件。

4. 播放舞蹈（在新终端）：

   source ~/g1_dance_ws/install/setup.bash ros2 run g1_traditional_dance trajectory_player

   观察Gazebo和RViz中的机器人，应该会按照预设的四个姿势进行运动。

5. 常见问题与排查思路

在开发过程中，你可能会遇到以下典型问题：

6. 进阶优化与工程实践

6.1 动作设计的艺术性与技术性结合

• 关键帧提取：研究真实舞蹈视频，使用关键帧提取工具或手动标注，获取人体关键点（如肩、肘、腕、髋、膝）的时空序列，再通过坐标转换映射到机器人关节空间。
• 运动曲线优化：不要简单线性插值。使用样条曲线（如五次样条）进行关节空间轨迹插值，使动作速度变化更符合“慢入慢出”的舞蹈美学。
• 全身协调：除了四肢，头颈和躯干的微小运动对表现力至关重要。为torso_yaw,head_pan,head_tilt等关节也设计细腻的动作。

6.2 仿真到实机的部署流程

1. 控制器切换：仿真中可能使用joint_trajectory_controller，而真机通常使用宇树提供的底层SDK或专用控制器。需要编写一个“适配器节点”，将标准的JointTrajectory消息转换为真机控制器能理解的指令（如特定的CAN消息或ROS2服务调用）。
2. 安全校验：在向真机发送任何轨迹前，必须在仿真中进行全速、全范围的测试。特别检查单脚支撑、快速旋转等易失稳动作。
3. 渐进式测试：先在真机上以慢速（如20%速度）回放轨迹，观察电机响应和机身振动。逐步提高速度，并实时监控关节电流和温度。
4. 急停与恢复：必须配置硬件急停开关和软件急停服务。一旦检测到异常力矩或姿态，立即停止所有电机并切换到阻尼模式。

6.3 系统集成与表演编排

• 状态机管理：使用SMACH或BehaviorTree.CPP等ROS2任务调度框架，将整个表演编排成状态机。状态包括：待机、预备姿势、舞蹈片段A、过渡、舞蹈片段B、谢幕、错误处理等。
• 多传感器融合：利用G1的IMU、足底力传感器和视觉摄像头。例如，根据IMU数据微调上身姿态以保持平衡；利用视觉进行定位，确保表演始终在舞台中央。
• 服饰的数字化呈现：虽然难以穿戴真实服饰，但可以在RViz或第三方渲染引擎中，为机器人模型绑定虚拟的服饰3D模型，并基于物理仿真（如Unity或Blender）计算服饰的飘动效果，与机器人动作同步渲染，生成最终表演视频。

6.4 代码管理与版本控制

• 参数配置化：将所有关节角度、时间参数、控制器增益等提取到YAML配置文件中。这样无需修改代码即可调整舞蹈动作。
• 轨迹版本化：使用Git管理trajectories/目录下的YAML文件。每次对动作的优化都应有明确的提交记录。
• 模块化设计：将舞蹈库（Dance Library）、播放器（Player）、安全监控（Safety Monitor）拆分为独立的ROS2节点或类，提高代码可复用性和可测试性。

从让宇树G1机器人摆出一个简单的拱手礼，到完成一段连贯的、富有韵味的传统舞蹈，这个过程充满了挑战与乐趣。它要求开发者不仅精通ROS2、MoveIt等机器人技术栈，还需要一点艺术感知力和将抽象概念转化为数学模型的耐心。本文提供了一套从环境搭建、动作规划、仿真验证到系统集成的完整路径。最关键的一步永远是动手实践：从官方文档和模型开始，让机器人在仿真中动起来，然后逐步加入你自己的创意。无论是为了学术研究、技术演示还是艺术创作，人形机器人的动作编排都是一个值得深入探索的领域，它正等待着更多开发者用代码为其注入灵魂。