博客地址：https://www.cnblogs.com/zylyehuo/
Unitree G1 模型文件下载地址(挑选自己需要的部分，本教程基于 g1_29dof.urdf (以及 .xml 和 meshes 文件夹))
有核心的 URDF 文件和 Meshes (STL 网格文件)
效果预览
工作空间结构
主要文件
display_and_gazebo.launch
<?xml version="1.0" encoding="UTF-8"?>
<launch><!-- 加载机器人URDF模型参数 --><param name="robot_description" textfile="$(find g1_description)/urdf/g1_29dof.urdf" /><!-- TF静态变换 --><node pkg="tf" type="static_transform_publisher" name="world_to_map" args="0 0 0 0 0 0 1 world map 10"/><!-- base_link和pelvis重合 - 静态 --><node pkg="tf" type="static_transform_publisher" name="base_link2pelvis" args="0.0 0.0 0.0 0.0 0.0 0.0 1 base_link pelvis 100" /><!-- 移除静态的map2base_link，改为动态TF --><node pkg="tf" type="static_transform_publisher" name="imu_in_torso2body_imu" args="0.0 0.0 0.0 0.0 0.0 0.0 1 imu_in_torso body_imu 100" /><!-- 机器人状态发布器 --><node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" /><!-- LinkStates到JointState的桥接，同时发布动态TF --><node name="link_states_bridge" pkg="g1_description" type="link_states_bridge.py" output="screen" /><!-- RViz --><node name="rviz" pkg="rviz" type="rviz" respawn="false" output="screen" /><!-- ============ Gazebo配置 ============ --><!-- 启动Gazebo --><include file="$(find gazebo_ros)/launch/empty_world.launch"><arg name="paused" value="false"/><arg name="use_sim_time" value="true"/><arg name="gui" value="true"/><arg name="headless" value="false"/></include><!-- 将机器人模型生成到Gazebo中 --><node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -z 0.79 -model g1_robot" output="screen" /></launch>link_states_bridge.py
#!/usr/bin/env python3import rospy
import math
from gazebo_msgs.msg import LinkStates
from sensor_msgs.msg import JointState
import numpy as np
from scipy.spatial.transform import Rotation
import threading
import tf
from geometry_msgs.msg import TransformStampedclass LinkStatesToJointState:def __init__(self):rospy.init_node('link_states_to_joint_state')# 订阅Gazebo的链接状态self.link_states_sub = rospy.Subscriber('/gazebo/link_states', LinkStates, self.link_states_callback, queue_size=1)# 发布joint_statesself.joint_states_pub = rospy.Publisher('/joint_states', JointState, queue_size=1)# 发布TF变换self.tf_broadcaster = tf.TransformBroadcaster()# 所有关节及其parent/child链接映射self.joints_info = {'left_hip_pitch_joint': ('pelvis', 'left_hip_pitch_link', [0, 1, 0]),'left_hip_roll_joint': ('left_hip_pitch_link', 'left_hip_roll_link', [1, 0, 0]),'left_hip_yaw_joint': ('left_hip_roll_link', 'left_hip_yaw_link', [0, 0, 1]),'left_knee_joint': ('left_hip_yaw_link', 'left_knee_link', [0, 1, 0]),'left_ankle_pitch_joint': ('left_knee_link', 'left_ankle_pitch_link', [0, 1, 0]),'left_ankle_roll_joint': ('left_ankle_pitch_link', 'left_ankle_roll_link', [1, 0, 0]),'right_hip_pitch_joint': ('pelvis', 'right_hip_pitch_link', [0, 1, 0]),'right_hip_roll_joint': ('right_hip_pitch_link', 'right_hip_roll_link', [1, 0, 0]),'right_hip_yaw_joint': ('right_hip_roll_link', 'right_hip_yaw_link', [0, 0, 1]),'right_knee_joint': ('right_hip_yaw_link', 'right_knee_link', [0, 1, 0]),'right_ankle_pitch_joint': ('right_knee_link', 'right_ankle_pitch_link', [0, 1, 0]),'right_ankle_roll_joint': ('right_ankle_pitch_link', 'right_ankle_roll_link', [1, 0, 0]),'waist_yaw_joint': ('pelvis', 'waist_yaw_link', [0, 0, 1]),'waist_roll_joint': ('waist_yaw_link', 'waist_roll_link', [1, 0, 0]),'waist_pitch_joint': ('waist_roll_link', 'torso_link', [0, 1, 0]),'left_shoulder_pitch_joint': ('torso_link', 'left_shoulder_pitch_link', [0, 1, 0]),'left_shoulder_roll_joint': ('left_shoulder_pitch_link', 'left_shoulder_roll_link', [1, 0, 0]),'left_shoulder_yaw_joint': ('left_shoulder_roll_link', 'left_shoulder_yaw_link', [0, 0, 1]),'left_elbow_joint': ('left_shoulder_yaw_link', 'left_elbow_link', [0, 1, 0]),'left_wrist_roll_joint': ('left_elbow_link', 'left_wrist_roll_link', [1, 0, 0]),'left_wrist_pitch_joint': ('left_wrist_roll_link', 'left_wrist_pitch_link', [0, 1, 0]),'left_wrist_yaw_joint': ('left_wrist_pitch_link', 'left_wrist_yaw_link', [0, 0, 1]),'right_shoulder_pitch_joint': ('torso_link', 'right_shoulder_pitch_link', [0, 1, 0]),'right_shoulder_roll_joint': ('right_shoulder_pitch_link', 'right_shoulder_roll_link', [1, 0, 0]),'right_shoulder_yaw_joint': ('right_shoulder_roll_link', 'right_shoulder_yaw_link', [0, 0, 1]),'right_elbow_joint': ('right_shoulder_yaw_link', 'right_elbow_link', [0, 1, 0]),'right_wrist_roll_joint': ('right_elbow_link', 'right_wrist_roll_link', [1, 0, 0]),'right_wrist_pitch_joint': ('right_wrist_roll_link', 'right_wrist_pitch_link', [0, 1, 0]),'right_wrist_yaw_joint': ('right_wrist_pitch_link', 'right_wrist_yaw_link', [0, 0, 1]),}self.last_msg = Noneself.lock = threading.Lock()self.initial_pelvis_z = None # 初始pelvis高度def link_states_callback(self, msg):with self.lock:self.last_msg = msgself.publish_joint_states(msg)self.publish_dynamic_tf(msg)def get_link_index(self, link_name, msg):"""获取链接在LinkStates中的索引"""full_name = f'g1_robot::{link_name}'try:return msg.name.index(full_name)except ValueError:return -1def get_relative_rotation(self, parent_pose, child_pose):"""计算从parent到child的相对旋转（四元数）"""# parent和child都是Pose消息，包含position和orientation# 计算相对四元数：q_rel = q_parent^-1 * q_childp_quat = [parent_pose.orientation.x, parent_pose.orientation.y, parent_pose.orientation.z, parent_pose.orientation.w]c_quat = [child_pose.orientation.x, child_pose.orientation.y, child_pose.orientation.z, child_pose.orientation.w]p_rot = Rotation.from_quat(p_quat)c_rot = Rotation.from_quat(c_quat)# 相对旋转rel_rot = p_rot.inv() * c_rotreturn rel_rotdef rotation_to_angle_around_axis(self, rotation, axis):"""从旋转矩阵中提取绕指定轴的旋转角度"""# 使用Rodrigues公式的逆angle = rotation.magnitude()if abs(angle) < 1e-6:return 0.0# 获取旋转轴rotvec = rotation.as_rotvec()rot_axis = rotvec / angle if angle > 1e-6 else [0, 0, 1]# 检查旋转轴是否与指定轴一致axis_norm = np.array(axis) / np.linalg.norm(axis)if np.dot(rot_axis, axis_norm) > 0.9:return angleelif np.dot(rot_axis, axis_norm) < -0.9:return -angleelse:# 使用欧拉角方法euler = rotation.as_euler('xyz')if axis == [1, 0, 0]:return euler[0]elif axis == [0, 1, 0]:return euler[1]elif axis == [0, 0, 1]:return euler[2]else:return 0.0def publish_joint_states(self, msg):"""发布joint_states消息"""joint_state = JointState()joint_state.header.stamp = rospy.Time.now()joint_state.name = list(self.joints_info.keys())joint_state.position = []joint_state.velocity = [0.0] * len(joint_state.name)joint_state.effort = [0.0] * len(joint_state.name)# 计算每个关节的角度for joint_name, (parent_name, child_name, axis) in self.joints_info.items():parent_idx = self.get_link_index(parent_name, msg)child_idx = self.get_link_index(child_name, msg)if parent_idx < 0 or child_idx < 0:joint_state.position.append(0.0)continue# 获取相对旋转rel_rot = self.get_relative_rotation(msg.pose[parent_idx], msg.pose[child_idx])# 提取绕指定轴的角度angle = self.rotation_to_angle_around_axis(rel_rot, axis)joint_state.position.append(angle)# 发布self.joint_states_pub.publish(joint_state)def publish_dynamic_tf(self, msg):"""发布动态TF：map → base_link（base_link和pelvis重合，跟随Gazebo中pelvis的实际位置和旋转）"""pelvis_idx = self.get_link_index('pelvis', msg)if pelvis_idx < 0:returnpelvis_pose = msg.pose[pelvis_idx]# map → base_link：base_link和pelvis重合，位置和旋转都来自Gazebo中的pelvisself.tf_broadcaster.sendTransform(translation=(pelvis_pose.position.x, pelvis_pose.position.y, pelvis_pose.position.z),rotation=(pelvis_pose.orientation.x, pelvis_pose.orientation.y, pelvis_pose.orientation.z, pelvis_pose.orientation.w),time=rospy.Time.now(),child='base_link',parent='map')if __name__ == '__main__':try:node = LinkStatesToJointState()rospy.spin()except rospy.ROSInterruptException:pass运行步骤
cd ~/g1_test_ws
catkin_make
source ./devel/setup.bash
roslaunch g1_description display_and_gazebo.launch
