如何基于Gazebo状态反馈在ROS1 noetic中实现Unitree G1关节联动与GazeboRViz联动？

博客地址：https://www.cnblogs.com/zylyehuo/ Unitree G1 模型文件下载地址(挑选自己需要的部分，本教程基于 g1_29dof.urdf (以及 .xml 和 meshes 文件夹)) 有核心的 URDF 文件和 Meshes (STL 网格文件) 为 Gazebo 中模型添加颜色参考：ROS1 noetic 中将 Unitree G1 的 URDF 导入 Gazebo/RViz 并非基于传统的 ros_control（通过插件控制关节），而是通过 Gazebo 状态反馈 -> 计算位姿 -> 映射到 TF 和 JointState 的方式实现的。 该系统跳过了 ros_control 控制器。 向 /joint_commands 发送 JointState 消息时，脚本会调用 Gazebo 的 /gazebo/set_model_configuration 服务。 这个服务直接“强行”设置 Gazebo 中物理模型的关节位置，类似于瞬移，而不是施加力矩。在调试规划算法时非常高效。 效果预览 工作空间结构 主要文件 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"/> <!-- NOTE: removed static base_link->pelvis to avoid TF duplication; link_states_bridge publishes dynamic map->base_link --> <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" /> <!-- NOTE: keep base_link->pelvis as a static zero transform so pelvis and base_link coincide --> <node pkg="tf" type="static_transform_publisher" name="base_link2pelvis" args="0 0 0 0 0 0 1 base_link pelvis 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" /> <!-- NOTE: controller parameters loading removed to avoid conflicts; using bridge(set_model_configuration) instead --> </launch> link_states_bridge.py #!/usr/bin/env python3 import rospy import math from gazebo_msgs.msg import LinkStates from sensor_msgs.msg import JointState from std_msgs.msg import Float64 import numpy as np from scipy.spatial.transform import Rotation import threading import tf from geometry_msgs.msg import TransformStamped from gazebo_msgs.srv import SetModelConfiguration class 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) # 订阅关节命令话题（用于控制Gazebo中的关节） self.joint_cmd_sub = rospy.Subscriber('/joint_commands', JointState, self.joint_cmd_callback, queue_size=1) # 发布joint_states self.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]), } # Gazebo set_model_configuration 服务代理（用于直接设置关节位置，替代ros_control） rospy.wait_for_service('/gazebo/set_model_configuration') self.set_model_config = rospy.ServiceProxy('/gazebo/set_model_configuration', SetModelConfiguration) self.last_msg = None self.lock = threading.Lock() self.initial_pelvis_z = None # 初始pelvis高度 # TF 发布节流参数 self.last_tf_time = rospy.Time(0) self.tf_min_interval = rospy.Duration(0.05) # 最小间隔 50ms self.last_pelvis_pose = None self.tf_pos_thresh = 0.005 # 5mm self.tf_rot_thresh = 0.01 # ~0.57deg rospy.loginfo("Link States to Joint State Bridge initialized") rospy.loginfo("Now using /gazebo/set_model_configuration to apply joint commands") rospy.loginfo("Publish JointState to /joint_commands to control joints") def link_states_callback(self, msg): with self.lock: self.last_msg = msg self.publish_joint_states(msg) self.publish_dynamic_tf(msg) def joint_cmd_callback(self, msg): """订阅关节命令话题，使用Gazebo服务设置关节位置（不依赖URDF transmission）""" try: if not msg.name or not msg.position: rospy.logwarn("Received empty joint command") return # 调用服务设置关节位置 model_name = 'g1_robot' urdf_param_name = 'robot_description' joint_names = list(msg.name) joint_positions = list(msg.position) rospy.loginfo(f"Setting joints via service: {joint_names} -> {joint_positions}") self.set_model_config(model_name, urdf_param_name, joint_names, joint_positions) except Exception as e: rospy.logerr(f"Failed to call set_model_configuration: {e}") 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 -1 def get_relative_rotation(self, parent_pose, child_pose): """计算从parent到child的相对旋转（四元数）""" p_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_rot return rel_rot def rotation_to_angle_around_axis(self, rotation, axis): 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 angle elif np.dot(rot_axis, axis_norm) < -0.9: return -angle else: 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.0 def publish_joint_states(self, msg): 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): pelvis_idx = self.get_link_index('pelvis', msg) if pelvis_idx < 0: return pelvis_pose = msg.pose[pelvis_idx] # 节流逻辑 now = rospy.Time.now() if self.last_pelvis_pose is not None: pos_diff = np.linalg.norm([ pelvis_pose.position.x - self.last_pelvis_pose.position.x, pelvis_pose.position.y - self.last_pelvis_pose.position.y, pelvis_pose.position.z - self.last_pelvis_pose.position.z ]) rot_diff = Rotation.from_quat([ pelvis_pose.orientation.x, pelvis_pose.orientation.y, pelvis_pose.orientation.z, pelvis_pose.orientation.w ]).inv() * Rotation.from_quat([ self.last_pelvis_pose.orientation.x, self.last_pelvis_pose.orientation.y, self.last_pelvis_pose.orientation.z, self.last_pelvis_pose.orientation.w ]) rot_diff_angle = rot_diff.magnitude() 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: return self.last_tf_time = now self.last_pelvis_pose = pelvis_pose translation = (pelvis_pose.position.x, pelvis_pose.position.y, pelvis_pose.position.z) rotation_q = (pelvis_pose.orientation.x, pelvis_pose.orientation.y, pelvis_pose.orientation.z, pelvis_pose.orientation.w) # 只发布 map -> base_link，让 base_link->pelvis 由静态发布器（launch）处理 self.tf_broadcaster.sendTransform( translation=translation, rotation=rotation_q, 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