ROS1 noetic 中将 Unitree G1 基于 Gazebo/RViz 关节联动【使用一个launch文件启动】

博客地址：https://www.cnblogs.com/zylyehuo/

Unitree G1 模型文件下载地址(挑选自己需要的部分，本教程基于 g1_29dof.urdf (以及 .xml 和 meshes 文件夹))

有核心的 URDF 文件和 Meshes (STL 网格文件)

效果预览

工作空间结构

主要文件

display_and_gazebo.launch

1. <?xml version="1.0" encoding="UTF-8"?>
2. <launch>
3.   
4.   <param name="robot_description" textfile="$(find g1_description)/urdf/g1_29dof.urdf" />
6.   
7.   <node pkg="tf" type="static_transform_publisher" name="world_to_map" args="0 0 0  0 0 0 1  world map 10"/>
8.   
9.   <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" />
10.   
11.   <node pkg="tf" type="static_transform_publisher" name="base_link2pelvis" args="0 0 0  0 0 0 1  base_link pelvis 100" />
13.   
14.   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
16.   
17.   <node name="link_states_bridge" pkg="g1_description" type="link_states_bridge.py" output="screen" />
19.   
20.   <node name="rviz" pkg="rviz" type="rviz" respawn="false" output="screen" />
22.   
23.   
24.   <include file="$(find gazebo_ros)/launch/empty_world.launch">
25.    
26.    
27.    
28.    
29.   </include>
31.   
32.   <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model"
33.     args="-param robot_description -urdf -z 0.79 -model g1_robot"
34.     output="screen" />
36.   
38. </launch>

复制代码

link_states_bridge.py

1. #!/usr/bin/env python3
3. import rospy
4. import math
5. from gazebo_msgs.msg import LinkStates
6. from sensor_msgs.msg import JointState
7. from std_msgs.msg import Float64
8. import numpy as np
9. from scipy.spatial.transform import Rotation
10. import threading
11. import tf
12. from geometry_msgs.msg import TransformStamped
13. from gazebo_msgs.srv import SetModelConfiguration
15. class LinkStatesToJointState:
16.     def __init__(self):
17.         rospy.init_node('link_states_to_joint_state')
18.         
19.         # 订阅Gazebo的链接状态
20.         self.link_states_sub = rospy.Subscriber('/gazebo/link_states', LinkStates, self.link_states_callback, queue_size=1)
21.         
22.         # 订阅关节命令话题（用于控制Gazebo中的关节）
23.         self.joint_cmd_sub = rospy.Subscriber('/joint_commands', JointState, self.joint_cmd_callback, queue_size=1)
24.         
25.         # 发布joint_states
26.         self.joint_states_pub = rospy.Publisher('/joint_states', JointState, queue_size=1)
27.         
28.         # 发布TF变换
29.         self.tf_broadcaster = tf.TransformBroadcaster()
30.         
31.         # 所有关节及其parent/child链接映射
32.         self.joints_info = {
33.             'left_hip_pitch_joint': ('pelvis', 'left_hip_pitch_link', [0, 1, 0]),
34.             'left_hip_roll_joint': ('left_hip_pitch_link', 'left_hip_roll_link', [1, 0, 0]),
35.             'left_hip_yaw_joint': ('left_hip_roll_link', 'left_hip_yaw_link', [0, 0, 1]),
36.             'left_knee_joint': ('left_hip_yaw_link', 'left_knee_link', [0, 1, 0]),
37.             'left_ankle_pitch_joint': ('left_knee_link', 'left_ankle_pitch_link', [0, 1, 0]),
38.             'left_ankle_roll_joint': ('left_ankle_pitch_link', 'left_ankle_roll_link', [1, 0, 0]),
39.             'right_hip_pitch_joint': ('pelvis', 'right_hip_pitch_link', [0, 1, 0]),
40.             'right_hip_roll_joint': ('right_hip_pitch_link', 'right_hip_roll_link', [1, 0, 0]),
41.             'right_hip_yaw_joint': ('right_hip_roll_link', 'right_hip_yaw_link', [0, 0, 1]),
42.             'right_knee_joint': ('right_hip_yaw_link', 'right_knee_link', [0, 1, 0]),
43.             'right_ankle_pitch_joint': ('right_knee_link', 'right_ankle_pitch_link', [0, 1, 0]),
44.             'right_ankle_roll_joint': ('right_ankle_pitch_link', 'right_ankle_roll_link', [1, 0, 0]),
45.             'waist_yaw_joint': ('pelvis', 'waist_yaw_link', [0, 0, 1]),
46.             'waist_roll_joint': ('waist_yaw_link', 'waist_roll_link', [1, 0, 0]),
47.             'waist_pitch_joint': ('waist_roll_link', 'torso_link', [0, 1, 0]),
48.             'left_shoulder_pitch_joint': ('torso_link', 'left_shoulder_pitch_link', [0, 1, 0]),
49.             'left_shoulder_roll_joint': ('left_shoulder_pitch_link', 'left_shoulder_roll_link', [1, 0, 0]),
50.             'left_shoulder_yaw_joint': ('left_shoulder_roll_link', 'left_shoulder_yaw_link', [0, 0, 1]),
51.             'left_elbow_joint': ('left_shoulder_yaw_link', 'left_elbow_link', [0, 1, 0]),
52.             'left_wrist_roll_joint': ('left_elbow_link', 'left_wrist_roll_link', [1, 0, 0]),
53.             'left_wrist_pitch_joint': ('left_wrist_roll_link', 'left_wrist_pitch_link', [0, 1, 0]),
54.             'left_wrist_yaw_joint': ('left_wrist_pitch_link', 'left_wrist_yaw_link', [0, 0, 1]),
55.             'right_shoulder_pitch_joint': ('torso_link', 'right_shoulder_pitch_link', [0, 1, 0]),
56.             'right_shoulder_roll_joint': ('right_shoulder_pitch_link', 'right_shoulder_roll_link', [1, 0, 0]),
57.             'right_shoulder_yaw_joint': ('right_shoulder_roll_link', 'right_shoulder_yaw_link', [0, 0, 1]),
58.             'right_elbow_joint': ('right_shoulder_yaw_link', 'right_elbow_link', [0, 1, 0]),
59.             'right_wrist_roll_joint': ('right_elbow_link', 'right_wrist_roll_link', [1, 0, 0]),
60.             'right_wrist_pitch_joint': ('right_wrist_roll_link', 'right_wrist_pitch_link', [0, 1, 0]),
61.             'right_wrist_yaw_joint': ('right_wrist_pitch_link', 'right_wrist_yaw_link', [0, 0, 1]),
62.         }
63.         
64.         # Gazebo set_model_configuration 服务代理（用于直接设置关节位置，替代ros_control）
65.         rospy.wait_for_service('/gazebo/set_model_configuration')
66.         self.set_model_config = rospy.ServiceProxy('/gazebo/set_model_configuration', SetModelConfiguration)
67.         
68.         self.last_msg = None
69.         self.lock = threading.Lock()
70.         self.initial_pelvis_z = None  # 初始pelvis高度
72.         # TF 发布节流参数
73.         self.last_tf_time = rospy.Time(0)
74.         self.tf_min_interval = rospy.Duration(0.05)  # 最小间隔 50ms
75.         self.last_pelvis_pose = None
76.         self.tf_pos_thresh = 0.005   # 5mm
77.         self.tf_rot_thresh = 0.01    # ~0.57deg
78.         
79.         rospy.loginfo("Link States to Joint State Bridge initialized")
80.         rospy.loginfo("Now using /gazebo/set_model_configuration to apply joint commands")
81.         rospy.loginfo("Publish JointState to /joint_commands to control joints")
82.    
83.     def link_states_callback(self, msg):
84.         with self.lock:
85.             self.last_msg = msg
86.             self.publish_joint_states(msg)
87.             self.publish_dynamic_tf(msg)
88.    
89.     def joint_cmd_callback(self, msg):
90.         """订阅关节命令话题，使用Gazebo服务设置关节位置（不依赖URDF transmission）"""
91.         try:
92.             if not msg.name or not msg.position:
93.                 rospy.logwarn("Received empty joint command")
94.                 return
95.             # 调用服务设置关节位置
96.             model_name = 'g1_robot'
97.             urdf_param_name = 'robot_description'
98.             joint_names = list(msg.name)
99.             joint_positions = list(msg.position)
100.             rospy.loginfo(f"Setting joints via service: {joint_names} -> {joint_positions}")
101.             self.set_model_config(model_name, urdf_param_name, joint_names, joint_positions)
102.         except Exception as e:
103.             rospy.logerr(f"Failed to call set_model_configuration: {e}")
104.    
105.     def get_link_index(self, link_name, msg):
106.         """获取链接在LinkStates中的索引"""
107.         full_name = f'g1_robot::{link_name}'
108.         try:
109.             return msg.name.index(full_name)
110.         except ValueError:
111.             return -1
112.    
113.     def get_relative_rotation(self, parent_pose, child_pose):
114.         """计算从parent到child的相对旋转（四元数）"""
115.         p_quat = [parent_pose.orientation.x, parent_pose.orientation.y,
116.                   parent_pose.orientation.z, parent_pose.orientation.w]
117.         c_quat = [child_pose.orientation.x, child_pose.orientation.y,
118.                   child_pose.orientation.z, child_pose.orientation.w]
119.         
120.         p_rot = Rotation.from_quat(p_quat)
121.         c_rot = Rotation.from_quat(c_quat)
122.         
123.         rel_rot = p_rot.inv() * c_rot
124.         
125.         return rel_rot
126.    
127.     def rotation_to_angle_around_axis(self, rotation, axis):
128.         angle = rotation.magnitude()
129.         
130.         if abs(angle) < 1e-6:
131.             return 0.0
132.         
133.         rotvec = rotation.as_rotvec()
134.         rot_axis = rotvec / angle if angle > 1e-6 else [0, 0, 1]
135.         
136.         axis_norm = np.array(axis) / np.linalg.norm(axis)
137.         
138.         if np.dot(rot_axis, axis_norm) > 0.9:
139.             return angle
140.         elif np.dot(rot_axis, axis_norm) < -0.9:
141.             return -angle
142.         else:
143.             euler = rotation.as_euler('xyz')
144.             if axis == [1, 0, 0]:
145.                 return euler[0]
146.             elif axis == [0, 1, 0]:
147.                 return euler[1]
148.             elif axis == [0, 0, 1]:
149.                 return euler[2]
150.             else:
151.                 return 0.0
152.    
153.     def publish_joint_states(self, msg):
154.         joint_state = JointState()
155.         joint_state.header.stamp = rospy.Time.now()
156.         joint_state.name = list(self.joints_info.keys())
157.         joint_state.position = []
158.         joint_state.velocity = [0.0] * len(joint_state.name)
159.         joint_state.effort = [0.0] * len(joint_state.name)
160.         
161.         for joint_name, (parent_name, child_name, axis) in self.joints_info.items():
162.             parent_idx = self.get_link_index(parent_name, msg)
163.             child_idx = self.get_link_index(child_name, msg)
164.             
165.             if parent_idx < 0 or child_idx < 0:
166.                 joint_state.position.append(0.0)
167.                 continue
168.             
169.             rel_rot = self.get_relative_rotation(msg.pose[parent_idx], msg.pose[child_idx])
170.             angle = self.rotation_to_angle_around_axis(rel_rot, axis)
171.             
172.             joint_state.position.append(angle)
173.         
174.         self.joint_states_pub.publish(joint_state)
175.    
176.     def publish_dynamic_tf(self, msg):
177.         pelvis_idx = self.get_link_index('pelvis', msg)
179.         if pelvis_idx < 0:
180.             return
182.         pelvis_pose = msg.pose[pelvis_idx]
184.         # 节流逻辑
185.         now = rospy.Time.now()
186.         if self.last_pelvis_pose is not None:
187.             pos_diff = np.linalg.norm([
188.                 pelvis_pose.position.x - self.last_pelvis_pose.position.x,
189.                 pelvis_pose.position.y - self.last_pelvis_pose.position.y,
190.                 pelvis_pose.position.z - self.last_pelvis_pose.position.z
191.             ])
192.             rot_diff = Rotation.from_quat([
193.                 pelvis_pose.orientation.x, pelvis_pose.orientation.y,
194.                 pelvis_pose.orientation.z, pelvis_pose.orientation.w
195.             ]).inv() * Rotation.from_quat([
196.                 self.last_pelvis_pose.orientation.x, self.last_pelvis_pose.orientation.y,
197.                 self.last_pelvis_pose.orientation.z, self.last_pelvis_pose.orientation.w
198.             ])
199.             rot_diff_angle = rot_diff.magnitude()
201.             if pos_diff < self.tf_pos_thresh and rot_diff_angle < self.tf_rot_thresh and (now - self.last_tf_time) < self.tf_min_interval:
202.                 return
204.         self.last_tf_time = now
205.         self.last_pelvis_pose = pelvis_pose
207.         translation = (pelvis_pose.position.x, pelvis_pose.position.y, pelvis_pose.position.z)
208.         rotation_q = (pelvis_pose.orientation.x, pelvis_pose.orientation.y, pelvis_pose.orientation.z, pelvis_pose.orientation.w)
210.         # 只发布 map -> base_link，让 base_link->pelvis 由静态发布器（launch）处理
211.         self.tf_broadcaster.sendTransform(
212.             translation=translation,
213.             rotation=rotation_q,
214.             time=rospy.Time.now(),
215.             child='base_link',
216.             parent='map'
217.         )
219. if __name__ == '__main__':
220.     try:
221.         node = LinkStatesToJointState()
222.         rospy.spin()
223.     except rospy.ROSInterruptException:
224.         pass

复制代码

运行步骤

1. cd ~/g1_test_ws

复制代码

1. catkin_make

复制代码

1. source ./devel/setup.bash

复制代码

1. roslaunch g1_description display_and_gazebo.launch

复制代码

来源：程序园用户自行投稿发布，如果侵权，请联系站长删除
免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！

感谢分享

yyds。多谢分享

这个好，看起来很实用

感谢发布原创作品，程序园因你更精彩

新版吗？好像是停更了吧。

感谢，下载保存了

感谢分享，学习下。

收藏一下   不知道什么时候能用到

yyds。多谢分享

喜欢鼓捣这些软件，现在用得少，谢谢分享！

过来提前占个楼

很好很强大  我过来先占个楼 待编辑

不错，里面软件多更新就更好了

感谢分享，下载保存了，貌似很强大

不错，里面软件多更新就更好了

感谢分享，学习下。

感谢分享，下载保存了，貌似很强大

收藏一下   不知道什么时候能用到

感谢，下载保存了