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
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<param name="robot_description" textfile="$(find g1_description)/urdf/g1_29dof.urdf" />
<node pkg="tf" type="static_transform_publisher" name="world_to_map" args="0 0 00 0 0 1world map 10"/>
<node pkg="tf" type="static_transform_publisher" name="imu_in_torso2body_imu" args="0.00.00.0 0.0 0.0 0.0 1imu_in_torso body_imu 100" />
<node pkg="tf" type="static_transform_publisher" name="base_link2pelvis" args="0 0 00 0 0 1base_link pelvis 100" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<node name="link_states_bridge" pkg="g1_description" type="link_states_bridge.py" output="screen" />
<node name="rviz" pkg="rviz" type="rviz" respawn="false" output="screen" />
<include file="$(find gazebo_ros)/launch/empty_world.launch">
</include>
<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 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', ),
'left_hip_roll_joint': ('left_hip_pitch_link', 'left_hip_roll_link', ),
'left_hip_yaw_joint': ('left_hip_roll_link', 'left_hip_yaw_link', ),
'left_knee_joint': ('left_hip_yaw_link', 'left_knee_link', ),
'left_ankle_pitch_joint': ('left_knee_link', 'left_ankle_pitch_link', ),
'left_ankle_roll_joint': ('left_ankle_pitch_link', 'left_ankle_roll_link', ),
'right_hip_pitch_joint': ('pelvis', 'right_hip_pitch_link', ),
'right_hip_roll_joint': ('right_hip_pitch_link', 'right_hip_roll_link', ),
'right_hip_yaw_joint': ('right_hip_roll_link', 'right_hip_yaw_link', ),
'right_knee_joint': ('right_hip_yaw_link', 'right_knee_link', ),
'right_ankle_pitch_joint': ('right_knee_link', 'right_ankle_pitch_link', ),
'right_ankle_roll_joint': ('right_ankle_pitch_link', 'right_ankle_roll_link', ),
'waist_yaw_joint': ('pelvis', 'waist_yaw_link', ),
'waist_roll_joint': ('waist_yaw_link', 'waist_roll_link', ),
'waist_pitch_joint': ('waist_roll_link', 'torso_link', ),
'left_shoulder_pitch_joint': ('torso_link', 'left_shoulder_pitch_link', ),
'left_shoulder_roll_joint': ('left_shoulder_pitch_link', 'left_shoulder_roll_link', ),
'left_shoulder_yaw_joint': ('left_shoulder_roll_link', 'left_shoulder_yaw_link', ),
'left_elbow_joint': ('left_shoulder_yaw_link', 'left_elbow_link', ),
'left_wrist_roll_joint': ('left_elbow_link', 'left_wrist_roll_link', ),
'left_wrist_pitch_joint': ('left_wrist_roll_link', 'left_wrist_pitch_link', ),
'left_wrist_yaw_joint': ('left_wrist_pitch_link', 'left_wrist_yaw_link', ),
'right_shoulder_pitch_joint': ('torso_link', 'right_shoulder_pitch_link', ),
'right_shoulder_roll_joint': ('right_shoulder_pitch_link', 'right_shoulder_roll_link', ),
'right_shoulder_yaw_joint': ('right_shoulder_roll_link', 'right_shoulder_yaw_link', ),
'right_elbow_joint': ('right_shoulder_yaw_link', 'right_elbow_link', ),
'right_wrist_roll_joint': ('right_elbow_link', 'right_wrist_roll_link', ),
'right_wrist_pitch_joint': ('right_wrist_roll_link', 'right_wrist_pitch_link', ),
'right_wrist_yaw_joint': ('right_wrist_pitch_link', 'right_wrist_yaw_link', ),
}
# 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
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 == :
return euler
elif axis == :
return euler
elif axis == :
return euler
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 = * len(joint_state.name)
joint_state.effort = * 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, msg.pose)
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
# 节流逻辑
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_wscatkin_makesource ./devel/setup.bashroslaunch g1_description display_and_gazebo.launch
来源：程序园用户自行投稿发布，如果侵权，请联系站长删除
免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！ 谢谢分享，辛苦了 这个好，看起来很实用 不错，里面软件多更新就更好了 谢谢分享，辛苦了 很好很强大我过来先占个楼 待编辑 感谢发布原创作品，程序园因你更精彩 这个好，看起来很实用 谢谢分享，辛苦了 新版吗？好像是停更了吧。 感谢发布原创作品，程序园因你更精彩 谢谢分享，试用一下 yyds。多谢分享 感谢，下载保存了 喜欢鼓捣这些软件，现在用得少，谢谢分享！ 这个好，看起来很实用 谢谢分享，试用一下 东西不错很实用谢谢分享 yyds。多谢分享 过来提前占个楼
页:
[1]
2