ROS1 noetic 中将 Unitree G1 基于 Gazebo/RViz 关节联动
-
RViz 只是“眼睛”，Gazebo 才是“身体”。
-
RViz 滑条 (joint_state_publisher)：它只是在广播“数据”：“我现在假设关节在 30 度”。RViz 听到了，就画给你看。
-
Gazebo：它模拟的是真实的物理电机。它听不到“假设”，它只接受“电机电压/扭矩指令”。
目前的连接是断开的： 滑条 GUI ---> /joint_states ---> RViz (更新画面) Gazebo (不仅没收到指令，而且现在的模型里连“电机”都没装，只有空壳)
要解决这个问题，需要做三件事
-
给 URDF 装电机：添加
标签。 -
配置控制器：生成 controllers.yaml。
-
写个转换桥梁：把滑条的信号“翻译”成电机指令发给 Gazebo。
效果预览
运行 rviz_gazebo.py
运行 rviz_gazebo_fixed.py
第一步：安装必要的 ROS 控制包
sudo apt-get install ros-noetic-ros-control ros-noetic-ros-controllers ros-noetic-gazebo-ros-control
第二步：运行一键配置脚本
~/g1_ws/src/g1_description/scripts/rviz_gazebo.py
这份生成的机器人会由于没有控制代码而倒地
#!/usr/bin/env python3
import os
import sys
import xml.etree.ElementTree as ET
# ================= 路径配置 =================
SCRIPT_DIR = os.path.dirname(os.path.abspath(__file__))
PKG_ROOT = os.path.dirname(SCRIPT_DIR)
PKG_NAME = "g1_description"
# 文件路径
SRC_URDF = os.path.join(PKG_ROOT, "urdf/g1_29dof.urdf")
DST_URDF = os.path.join(PKG_ROOT, "urdf/g1_complete.urdf")
YAML_FILE = os.path.join(PKG_ROOT, "config/controllers.yaml")
BRIDGE_SCRIPT = os.path.join(PKG_ROOT, "scripts/bridge_node.py")
LAUNCH_GAZEBO = os.path.join(PKG_ROOT, "launch/1_gazebo.launch")
LAUNCH_RVIZ = os.path.join(PKG_ROOT, "launch/2_rviz.launch")
# 颜色映射
COLOR_MAP = {
"white": "Gazebo/Grey",
"grey": "Gazebo/Grey",
"dark": "Gazebo/DarkGrey",
"black": "Gazebo/FlatBlack"
}
print(f"正在生成【全物理走路调试版】环境...")
def generate_files():
# 1. 目录检查
os.makedirs(os.path.join(PKG_ROOT, "config"), exist_ok=True)
os.makedirs(os.path.join(PKG_ROOT, "launch"), exist_ok=True)
if not os.path.exists(SRC_URDF):
print(f"错误: 找不到 {SRC_URDF}")
sys.exit(1)
tree = ET.parse(SRC_URDF)
root = tree.getroot()
joint_names = []
# ================= 1. URDF 物理配置 (Floating Base) =================
# 1.1 移除所有 World 固定锚点 (彻底自由)
for link in root.findall('link'):
if link.get('name') == 'world': root.remove(link)
for joint in root.findall('joint'):
if joint.get('name') == 'world_anchor': root.remove(joint)
# 1.2 确保 base_link 存在 (作为浮动基座的根)
has_base = False
for link in root.findall('link'):
if link.get('name') == 'base_link': has_base = True
if not has_base:
base_link = ET.Element('link', name='base_link')
# 给 base_link 一个微小的惯性，防止 Gazebo 忽略它
inertial = ET.SubElement(base_link, 'inertial')
ET.SubElement(inertial, 'mass', value="0.01")
ET.SubElement(inertial, 'inertia', ixx="0.001", ixy="0", ixz="0", iyy="0.001", iyz="0", izz="0.001")
root.insert(0, base_link)
# 连接 base_link -> pelvis
joint = ET.Element('joint', name='base_to_pelvis', type='fixed')
ET.SubElement(joint, 'parent', link='base_link')
ET.SubElement(joint, 'child', link='pelvis')
ET.SubElement(joint, 'origin', xyz="0 0 0", rpy="0 0 0")
root.insert(1, joint)
# 1.3 物理参数精细化 (摩擦力与接触刚度)
# 清理所有旧的 gazebo 标签
for gazebo in root.findall('gazebo'):
root.remove(gazebo)
for link in root.findall('link'):
name = link.get('name')
# A. 惯性修复
inertial = link.find('inertial')
if inertial:
mass = inertial.find('mass')
if mass and float(mass.get('value')) < 0.05: mass.set('value', '0.05')
inertia = inertial.find('inertia')
if inertia:
for ax in ['ixx','iyy','izz']:
if float(inertia.get(ax)) < 0.0001: inertia.set(ax, '0.0001')
# B. Gazebo 属性生成
gz = ET.SubElement(root, 'gazebo', reference=name)
# 开启重力！(这是走路的前提)
ET.SubElement(gz, 'turnGravityOff').text = 'false'
ET.SubElement(gz, 'selfCollide').text = 'false' # 暂时关闭自碰撞防止炸机
# 颜色匹配
visual = link.find('visual')
mat_name = "white"
if visual:
mat = visual.find('material')
if mat is not None: mat_name = mat.get('name', 'white').lower()
ET.SubElement(gz, 'material').text = COLOR_MAP.get(mat_name, "Gazebo/Grey")
# C. 【关键】脚底摩擦力增强
# 假设 link 名字里带 ankle_roll 的是脚底接触件
if "ankle_roll" in name:
# 高摩擦力，防止打滑
ET.SubElement(gz, 'mu1').text = "1.5"
ET.SubElement(gz, 'mu2').text = "1.5"
# 接触刚度 (kp) 和 阻尼 (kd)
# kp 越大越硬，kd 越大越不弹
ET.SubElement(gz, 'kp').text = "1000000.0"
ET.SubElement(gz, 'kd').text = "100.0"
# 最小穿透深度，防止抖动
ET.SubElement(gz, 'minDepth').text = "0.001"
ET.SubElement(gz, 'maxVel').text = "0.0" # 防止高速弹射
# ================= 2. 插件与电机 =================
# ros_control
gz_c = ET.SubElement(root, 'gazebo')
pl_c = ET.SubElement(gz_c, 'plugin', name="gazebo_ros_control", filename="libgazebo_ros_control.so")
ET.SubElement(pl_c, 'robotNamespace').text = f"/{PKG_NAME}"
ET.SubElement(pl_c, 'legacyModeNS').text = "true"
# P3D (真值反馈，用于做闭环控制)
gz_p3d = ET.SubElement(root, 'gazebo')
pl_p3d = ET.SubElement(gz_p3d, 'plugin', name="p3d_base_controller", filename="libgazebo_ros_p3d.so")
ET.SubElement(pl_p3d, 'alwaysOn').text = "true"
ET.SubElement(pl_p3d, 'updateRate').text = "100.0"
ET.SubElement(pl_p3d, 'bodyName').text = "pelvis"
ET.SubElement(pl_p3d, 'topicName').text = "ground_truth_odom"
ET.SubElement(pl_p3d, 'frameName').text = "world"
ET.SubElement(pl_p3d, 'xyzOffsets').text = "0 0 0"
ET.SubElement(pl_p3d, 'rpyOffsets').text = "0 0 0"
# 电机接口
for joint in root.findall('joint'):
if joint.get('type') in ['revolute', 'continuous', 'prismatic']:
name = joint.get('name')
joint_names.append(name)
# 去重
is_dup = False
for tr in root.findall('transmission'):
if tr.get('name') == f"{name}_trans": is_dup = True
if not is_dup:
trans = ET.SubElement(root, 'transmission', name=f"{name}_trans")
ET.SubElement(trans, 'type').text = "transmission_interface/SimpleTransmission"
j = ET.SubElement(trans, 'joint', name=name)
ET.SubElement(j, 'hardwareInterface').text = "hardware_interface/PositionJointInterface"
act = ET.SubElement(trans, 'actuator', name=f"{name}_motor")
ET.SubElement(act, 'mechanicalReduction').text = "1"
ET.SubElement(act, 'hardwareInterface').text = "hardware_interface/PositionJointInterface"
tree.write(DST_URDF, encoding='utf-8', xml_declaration=True)
# ================= 3. PID 参数 (支撑级) =================
with open(YAML_FILE, 'w') as f:
f.write(f"{PKG_NAME}:\n")
f.write(" joint_state_controller:\n")
f.write(" type: joint_state_controller/JointStateController\n")
f.write(" publish_rate: 100\n\n")
for jn in joint_names:
f.write(f" {jn}_controller:\n")
f.write(" type: position_controllers/JointPositionController\n")
f.write(f" joint: {jn}\n")
# 这里的 PID 是控制器层面的，可以稍大
f.write(" pid: {p: 800.0, i: 0.0, d: 20.0}\n")
# Gazebo 物理引擎 PID (核心)
f.write("\n gazebo_ros_control:\n pid_gains:\n")
for jn in joint_names:
if "ankle" in jn or "knee" in jn or "hip" in jn:
# 腿部：需要大力矩支撑体重，但阻尼要适中防止震荡
# P=800~1000 足够支撑 G1 这种体型的机器人
f.write(f" {jn}: {{p: 1000.0, i: 0.1, d: 50.0}}\n")
else:
# 手臂：不需要太大
f.write(f" {jn}: {{p: 300.0, i: 0.0, d: 5.0}}\n")
# ================= 4. Bridge Script =================
with open(BRIDGE_SCRIPT, 'w') as f:
f.write(f"""#!/usr/bin/env python3
import rospy
from sensor_msgs.msg import JointState
from std_msgs.msg import Float64
class Bridge:
def __init__(self):
rospy.init_node('rviz_bridge')
self.pubs = {{}}
self.ns = "/{PKG_NAME}"
rospy.Subscriber("/gui_commands", JointState, self.cb)
def cb(self, msg):
for i, n in enumerate(msg.name):
if n not in self.pubs: self.pubs[n] = rospy.Publisher(f"{{self.ns}}/{{n}}_controller/command", Float64, queue_size=10)
self.pubs[n].publish(msg.position[i])
if __name__ == '__main__': Bridge(); rospy.spin()
""")
os.chmod(BRIDGE_SCRIPT, 0o755)
# ================= 5. Launch Gazebo (Walk Ready) =================
ctl_str = " ".join([f"{jn}_controller" for jn in joint_names])
urdf_rel = "urdf/g1_complete.urdf"
yaml_rel = "config/controllers.yaml"
with open(LAUNCH_GAZEBO, 'w') as f:
f.write(f"""<launch>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="paused" value="true"/> <arg name="gui" value="true"/>
</include>
<param name="robot_description" textfile="$(find {PKG_NAME})/{urdf_rel}" />
<rosparam file="$(find {PKG_NAME})/{yaml_rel}" command="load"/>
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model"
args="-file $(find {PKG_NAME})/{urdf_rel} -urdf -z 0.85 -model g1_robot
-J left_hip_pitch_joint -0.45
-J left_knee_joint 0.9
-J left_ankle_pitch_joint -0.45
-J right_hip_pitch_joint -0.45
-J right_knee_joint 0.9
-J right_ankle_pitch_joint -0.45"
output="screen" />
<node name="ctl_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" ns="/{PKG_NAME}" args="joint_state_controller {ctl_str}"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen">
<remap from="/joint_states" to="/{PKG_NAME}/joint_states" />
</node>
<node name="bridge" pkg="{PKG_NAME}" type="bridge_node.py" output="screen" />
</launch>""")
# ================= 6. Launch RViz =================
with open(LAUNCH_RVIZ, 'w') as f:
f.write(f"""<launch>
<param name="robot_description" textfile="$(find {PKG_NAME})/{urdf_rel}" />
<node name="jsp_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui">
<remap from="/joint_states" to="/gui_commands"/>
</node>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find {PKG_NAME})/launch/g1.rviz" />
</launch>""")
print("\n[完成] 调试环境已就绪！")
print("-------------------------------------------------------")
print("1. python3 scripts/rviz_gazebo.py")
print("2. cd ~/g1_ws && catkin_make && source devel/setup.bash")
print("3. roslaunch g1_description 1_gazebo.launch")
print(" -> 注意：Gazebo 启动是暂停的。点击 Play 后，机器人会下落到地面。")
print(" -> 如果参数合适，它会保持蹲姿平衡；如果不合适，它会倒。")
print("-------------------------------------------------------")
if __name__ == "__main__":
generate_files()
~/g1_ws/src/g1_description/scripts/rviz_gazebo_fixed.py
这份生成的机器人会固定在世界系中
#!/usr/bin/env python3
import os
import sys
import xml.etree.ElementTree as ET
# ================= 路径配置 =================
SCRIPT_DIR = os.path.dirname(os.path.abspath(__file__))
PKG_ROOT = os.path.dirname(SCRIPT_DIR)
PKG_NAME = "g1_description"
# 文件路径
SRC_URDF = os.path.join(PKG_ROOT, "urdf/g1_29dof.urdf")
DST_URDF = os.path.join(PKG_ROOT, "urdf/g1_complete.urdf")
YAML_FILE = os.path.join(PKG_ROOT, "config/controllers.yaml")
BRIDGE_SCRIPT = os.path.join(PKG_ROOT, "scripts/bridge_node.py")
LAUNCH_GAZEBO = os.path.join(PKG_ROOT, "launch/1_gazebo.launch")
LAUNCH_RVIZ = os.path.join(PKG_ROOT, "launch/2_rviz.launch")
# 颜色映射表 (URDF材质名 -> Gazebo材质名)
COLOR_MAP = {
"white": "Gazebo/Grey", # 银灰色
"grey": "Gazebo/Grey",
"dark": "Gazebo/DarkGrey", # 深黑色
"black": "Gazebo/FlatBlack"
}
print(f"正在生成【完美静止 + 颜色修复版】配置文件...")
def generate_files():
os.makedirs(os.path.join(PKG_ROOT, "config"), exist_ok=True)
os.makedirs(os.path.join(PKG_ROOT, "launch"), exist_ok=True)
if not os.path.exists(SRC_URDF):
print(f"错误: 找不到 {SRC_URDF}")
sys.exit(1)
tree = ET.parse(SRC_URDF)
root = tree.getroot()
joint_names = []
# ================= 1. 结构修复 (Base Link & World Anchor) =================
# 1.1 补全 base_link
has_base = False
for link in root.findall('link'):
if link.get('name') == 'base_link': has_base = True
if not has_base:
base_link = ET.Element('link', name='base_link')
root.insert(0, base_link)
joint = ET.Element('joint', name='base_to_pelvis', type='fixed')
ET.SubElement(joint, 'parent', link='base_link')
ET.SubElement(joint, 'child', link='pelvis')
ET.SubElement(joint, 'origin', xyz="0 0 0", rpy="0 0 0")
root.insert(1, joint)
# 1.2 添加 World 锚点 (固定在 0.79m)
has_world = False
for link in root.findall('link'):
if link.get('name') == 'world': has_world = True
if not has_world:
world_link = ET.Element('link', name='world')
root.insert(0, world_link)
anchor = ET.Element('joint', name='world_anchor', type='fixed')
ET.SubElement(anchor, 'parent', link='world')
ET.SubElement(anchor, 'child', link='base_link')
ET.SubElement(anchor, 'origin', xyz="0 0 0.79", rpy="0 0 0")
root.insert(1, anchor)
# ================= 2. 颜色与物理修复 (核心) =================
# 先清理所有现有的 gazebo 标签，避免冲突
for gazebo in root.findall('gazebo'):
root.remove(gazebo)
# 遍历所有 Link，重新生成属性
for link in root.findall('link'):
name = link.get('name')
if name == 'world': continue
# --- A. 物理惯性清洗 ---
inertial = link.find('inertial')
if inertial:
mass = inertial.find('mass')
if mass:
if float(mass.get('value')) < 0.01: mass.set('value', '0.01')
inertia = inertial.find('inertia')
if inertia:
for ax in ['ixx','iyy','izz']:
if float(inertia.get(ax)) < 0.001: inertia.set(ax, '0.001')
# --- B. 生成 Gazebo 标签 (颜色 + 物理) ---
gz = ET.SubElement(root, 'gazebo', reference=name)
# B1. 物理属性 (无重力，无碰撞)
ET.SubElement(gz, 'turnGravityOff').text = 'true'
ET.SubElement(gz, 'selfCollide').text = 'false'
# B2. 颜色自动匹配
# 查找该 link 下的 visual -> material -> name
visual = link.find('visual')
mat_name = "white" # 默认值
if visual:
mat = visual.find('material')
if mat is not None:
mat_name = mat.get('name', 'white').lower()
# 根据字典映射颜色
gz_color = COLOR_MAP.get(mat_name, "Gazebo/Grey")
ET.SubElement(gz, 'material').text = gz_color
# ================= 3. 插件与电机 =================
gz_c = ET.SubElement(root, 'gazebo')
pl_c = ET.SubElement(gz_c, 'plugin', name="gazebo_ros_control", filename="libgazebo_ros_control.so")
ET.SubElement(pl_c, 'robotNamespace').text = f"/{PKG_NAME}"
ET.SubElement(pl_c, 'legacyModeNS').text = "true"
for joint in root.findall('joint'):
if joint.get('type') in ['revolute', 'continuous', 'prismatic']:
name = joint.get('name')
joint_names.append(name)
# 检查重复
is_dup = False
for tr in root.findall('transmission'):
if tr.get('name') == f"{name}_trans": is_dup = True
if not is_dup:
trans = ET.SubElement(root, 'transmission', name=f"{name}_trans")
ET.SubElement(trans, 'type').text = "transmission_interface/SimpleTransmission"
j = ET.SubElement(trans, 'joint', name=name)
ET.SubElement(j, 'hardwareInterface').text = "hardware_interface/PositionJointInterface"
act = ET.SubElement(trans, 'actuator', name=f"{name}_motor")
ET.SubElement(act, 'mechanicalReduction').text = "1"
ET.SubElement(act, 'hardwareInterface').text = "hardware_interface/PositionJointInterface"
tree.write(DST_URDF, encoding='utf-8', xml_declaration=True)
# ================= 4. PID Config (柔和防抖) =================
with open(YAML_FILE, 'w') as f:
f.write(f"{PKG_NAME}:\n")
f.write(" joint_state_controller:\n")
f.write(" type: joint_state_controller/JointStateController\n")
f.write(" publish_rate: 100\n\n")
for jn in joint_names:
f.write(f" {jn}_controller:\n")
f.write(" type: position_controllers/JointPositionController\n")
f.write(f" joint: {jn}\n")
# 这里的 PID 可以小一点，因为 Gazebo 物理层有自己的 PID
f.write(" pid: {p: 100.0, i: 0.0, d: 1.0}\n")
# Gazebo 物理 PID (P=50, D=0.5 足够驱动失重关节)
f.write("\n gazebo_ros_control:\n pid_gains:\n")
for jn in joint_names:
f.write(f" {jn}: {{p: 50.0, i: 0.0, d: 0.5}}\n")
# ================= 5. Bridge Script =================
with open(BRIDGE_SCRIPT, 'w') as f:
f.write(f"""#!/usr/bin/env python3
import rospy
from sensor_msgs.msg import JointState
from std_msgs.msg import Float64
class Bridge:
def __init__(self):
rospy.init_node('rviz_bridge')
self.pubs = {{}}
self.ns = "/{PKG_NAME}"
rospy.Subscriber("/gui_commands", JointState, self.cb)
def cb(self, msg):
for i, n in enumerate(msg.name):
if n not in self.pubs: self.pubs[n] = rospy.Publisher(f"{{self.ns}}/{{n}}_controller/command", Float64, queue_size=10)
self.pubs[n].publish(msg.position[i])
if __name__ == '__main__': Bridge(); rospy.spin()
""")
os.chmod(BRIDGE_SCRIPT, 0o755)
# ================= 6. Launch Gazebo =================
ctl_str = " ".join([f"{jn}_controller" for jn in joint_names])
urdf_rel = "urdf/g1_complete.urdf"
yaml_rel = "config/controllers.yaml"
with open(LAUNCH_GAZEBO, 'w') as f:
f.write(f"""<launch>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="paused" value="false"/>
<arg name="gui" value="true"/>
</include>
<param name="robot_description" textfile="$(find {PKG_NAME})/{urdf_rel}" />
<rosparam file="$(find {PKG_NAME})/{yaml_rel}" command="load"/>
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model"
args="-file $(find {PKG_NAME})/{urdf_rel} -urdf -model g1_robot"
output="screen" />
<node name="ctl_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" ns="/{PKG_NAME}" args="joint_state_controller {ctl_str}"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen">
<remap from="/joint_states" to="/{PKG_NAME}/joint_states" />
</node>
<node name="bridge" pkg="{PKG_NAME}" type="bridge_node.py" output="screen" />
</launch>""")
# ================= 7. Launch RViz =================
with open(LAUNCH_RVIZ, 'w') as f:
f.write(f"""<launch>
<param name="robot_description" textfile="$(find {PKG_NAME})/{urdf_rel}" />
<node name="jsp_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui">
<remap from="/joint_states" to="/gui_commands"/>
</node>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find {PKG_NAME})/launch/g1.rviz" />
</launch>""")
print("-------------------------------------------------------")
print("1. python3 scripts/rviz_gazebo.py")
print("2. cd ~/g1_ws && catkin_make && source devel/setup.bash")
print("3. roslaunch g1_description 1_gazebo.launch")
print("4. roslaunch g1_description 2_rviz.launch")
print("-------------------------------------------------------")
if __name__ == "__main__":
generate_files()
第三步：运行配置脚本
python3 ~/g1_ws/src/g1_description/scripts/rviz_gazebo.py
# python3 ~/g1_ws/src/g1_description/scripts/rviz_gazebo_fixed.py
第四步：编译并运行
cd ~/g1_ws
catkin_make
source devel/setup.bash
终端 1 (仿真)
roslaunch g1_description 1_gazebo.launch
终端 2 (控制)
roslaunch g1_description 2_rviz.launch
浙公网安备 33010602011771号