ROS1 noetic 中将 Unitree G1 基于 Gazebo/RViz 关节联动【基于 ros_control】

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

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

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

为 Gazebo 中模型添加颜色参考：ROS1 noetic 中将 Unitree G1 的 URDF 导入 Gazebo/RViz

效果预览

工作空间结构

在URDF中添加重要标签

给 URDF 装电机：添加 <transmission> 标签。

添加控制器： gazebo_ros_control

<robot name="g1_29dof"><material name="dark"><color rgba="0.2 0.2 0.2 1"/></material><material name="white"><color rgba="0.7 0.7 0.7 1"/></material><mujoco><compiler meshdir="meshes" discardvisual="false"/></mujoco><!-- [CAUTION] uncomment when convert to mujoco --><!-- <link name="world"></link><joint name="floating_base_joint" type="floating"><parent link="world"/><child link="pelvis"/></joint> --><link name="pelvis"><inertial><origin xyz="0 0 -0.07605" rpy="0 0 0"/><mass value="3.813"/><inertia ixx="0.010549" ixy="0" ixz="2.1E-06" iyy="0.0093089" iyz="0" izz="0.0079184"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/pelvis.STL"/></geometry><material name="dark"/></visual></link><link name="pelvis_contour_link"><inertial><origin xyz="0 0 0" rpy="0 0 0"/><mass value="0.001"/><inertia ixx="1e-7" ixy="0" ixz="0" iyy="1e-7" iyz="0" izz="1e-7"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/pelvis_contour_link.STL"/></geometry><material name="white"/></visual><collision><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/pelvis_contour_link.STL"/></geometry></collision></link><joint name="pelvis_contour_joint" type="fixed"><parent link="pelvis"/><child link="pelvis_contour_link"/></joint><!-- Legs --><link name="left_hip_pitch_link"><inertial><origin xyz="0.002741 0.047791 -0.02606" rpy="0 0 0"/><mass value="1.35"/><inertia ixx="0.001811" ixy="3.68E-05" ixz="-3.44E-05" iyy="0.0014193" iyz="0.000171" izz="0.0012812"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/left_hip_pitch_link.STL"/></geometry><material name="dark"/></visual><collision><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/left_hip_pitch_link.STL"/></geometry></collision></link><joint name="left_hip_pitch_joint" type="revolute"><origin xyz="0 0.064452 -0.1027" rpy="0 0 0"/><parent link="pelvis"/><child link="left_hip_pitch_link"/><axis xyz="0 1 0"/><limit lower="-2.5307" upper="2.8798" effort="88" velocity="32"/></joint><link name="left_hip_roll_link"><inertial><origin xyz="0.029812 -0.001045 -0.087934" rpy="0 0 0"/><mass value="1.52"/><inertia ixx="0.0023773" ixy="-3.8E-06" ixz="-0.0003908" iyy="0.0024123" iyz="1.84E-05" izz="0.0016595"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/left_hip_roll_link.STL"/></geometry><material name="white"/></visual><collision><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/left_hip_roll_link.STL"/></geometry></collision></link><joint name="left_hip_roll_joint" type="revolute"><origin xyz="0 0.052 -0.030465" rpy="0 -0.1749 0"/><parent link="left_hip_pitch_link"/><child link="left_hip_roll_link"/><axis xyz="1 0 0"/><limit lower="-0.5236" upper="2.9671" effort="88" velocity="32"/></joint><link name="left_hip_yaw_link"><inertial><origin xyz="-0.057709 -0.010981 -0.15078" rpy="0 0 0"/><mass value="1.702"/><inertia ixx="0.0057774" ixy="-0.0005411" ixz="-0.0023948" iyy="0.0076124" iyz="-0.0007072" izz="0.003149"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/left_hip_yaw_link.STL"/></geometry><material name="white"/></visual><collision><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/left_hip_yaw_link.STL"/></geometry></collision></link><joint name="left_hip_yaw_joint" type="revolute"><origin xyz="0.025001 0 -0.12412" rpy="0 0 0"/><parent link="left_hip_roll_link"/><child link="left_hip_yaw_link"/><axis xyz="0 0 1"/><limit lower="-2.7576" upper="2.7576" effort="88" velocity="32"/></joint><link name="left_knee_link"><inertial><origin xyz="0.005457 0.003964 -0.12074" rpy="0 0 0"/><mass value="1.932"/><inertia ixx="0.011329" ixy="4.82E-05" ixz="-4.49E-05" iyy="0.011277" iyz="-0.0007146" izz="0.0015168"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/left_knee_link.STL"/></geometry><material name="white"/></visual><collision><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/left_knee_link.STL"/></geometry></collision></link><joint name="left_knee_joint" type="revolute"><origin xyz="-0.078273 0.0021489 -0.17734" rpy="0 0.1749 0"/><parent link="left_hip_yaw_link"/><child link="left_knee_link"/><axis xyz="0 1 0"/><limit lower="-0.087267" upper="2.8798" effort="139" velocity="20"/></joint><link name="left_ankle_pitch_link"><inertial><origin xyz="-0.007269 0 0.011137" rpy="0 0 0"/><mass value="0.074"/><inertia ixx="8.4E-06" ixy="0" ixz="-2.9E-06" iyy="1.89E-05" iyz="0" izz="1.26E-05"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/left_ankle_pitch_link.STL"/></geometry><material name="white"/></visual><collision><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/left_ankle_pitch_link.STL"/></geometry></collision></link><joint name="left_ankle_pitch_joint" type="revolute"><origin xyz="0 -9.4445E-05 -0.30001" rpy="0 0 0"/><parent link="left_knee_link"/><child link="left_ankle_pitch_link"/><axis xyz="0 1 0"/><limit lower="-0.87267" upper="0.5236" effort="35" velocity="30"/></joint><link name="left_ankle_roll_link"><inertial><origin xyz="0.026505 0 -0.016425" rpy="0 0 0"/><mass value="0.608"/><inertia ixx="0.0002231" ixy="2E-07" ixz="8.91E-05" iyy="0.0016161" iyz="-1E-07" izz="0.0016667"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/left_ankle_roll_link.STL"/></geometry><material name="dark"/></visual><collision><origin xyz="-0.05 0.025 -0.03" rpy="0 0 0"/><geometry><sphere radius="0.005"/></geometry></collision><collision><origin xyz="-0.05 -0.025 -0.03" rpy="0 0 0"/><geometry><sphere radius="0.005"/></geometry></collision><collision><origin xyz="0.12 0.03 -0.03" rpy="0 0 0"/><geometry><sphere radius="0.005"/></geometry></collision><collision><origin xyz="0.12 -0.03 -0.03" rpy="0 0 0"/><geometry><sphere radius="0.005"/></geometry></collision></link><joint name="left_ankle_roll_joint" type="revolute"><origin xyz="0 0 -0.017558" rpy="0 0 0"/><parent link="left_ankle_pitch_link"/><child link="left_ankle_roll_link"/><axis xyz="1 0 0"/><limit lower="-0.2618" upper="0.2618" effort="35" velocity="30"/></joint><link name="right_hip_pitch_link"><inertial><origin xyz="0.002741 -0.047791 -0.02606" rpy="0 0 0"/><mass value="1.35"/><inertia ixx="0.001811" ixy="-3.68E-05" ixz="-3.44E-05" iyy="0.0014193" iyz="-0.000171" izz="0.0012812"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/right_hip_pitch_link.STL"/></geometry><material name="dark"/></visual><collision><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/right_hip_pitch_link.STL"/></geometry></collision></link><joint name="right_hip_pitch_joint" type="revolute"><origin xyz="0 -0.064452 -0.1027" rpy="0 0 0"/><parent link="pelvis"/><child link="right_hip_pitch_link"/><axis xyz="0 1 0"/><limit lower="-2.5307" upper="2.8798" effort="88" velocity="32"/></joint><link name="right_hip_roll_link"><inertial><origin xyz="0.029812 0.001045 -0.087934" rpy="0 0 0"/><mass value="1.52"/><inertia ixx="0.0023773" ixy="3.8E-06" ixz="-0.0003908" iyy="0.0024123" iyz="-1.84E-05" izz="0.0016595"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/right_hip_roll_link.STL"/></geometry><material name="white"/></visual><collision><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/right_hip_roll_link.STL"/></geometry></collision></link><joint name="right_hip_roll_joint" type="revolute"><origin xyz="0 -0.052 -0.030465" rpy="0 -0.1749 0"/><parent link="right_hip_pitch_link"/><child link="right_hip_roll_link"/><axis xyz="1 0 0"/><limit lower="-2.9671" upper="0.5236" effort="88" velocity="32"/></joint><link name="right_hip_yaw_link"><inertial><origin xyz="-0.057709 0.010981 -0.15078" rpy="0 0 0"/><mass value="1.702"/><inertia ixx="0.0057774" ixy="0.0005411" ixz="-0.0023948" iyy="0.0076124" iyz="0.0007072" izz="0.003149"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/right_hip_yaw_link.STL"/></geometry><material name="white"/></visual><collision><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/right_hip_yaw_link.STL"/></geometry></collision></link><joint name="right_hip_yaw_joint" type="revolute"><origin xyz="0.025001 0 -0.12412" rpy="0 0 0"/><parent link="right_hip_roll_link"/><child link="right_hip_yaw_link"/><axis xyz="0 0 1"/><limit lower="-2.7576" upper="2.7576" effort="88" velocity="32"/></joint><link name="right_knee_link"><inertial><origin xyz="0.005457 -0.003964 -0.12074" rpy="0 0 0"/><mass value="1.932"/><inertia ixx="0.011329" ixy="-4.82E-05" ixz="4.49E-05" iyy="0.011277" iyz="0.0007146" izz="0.0015168"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/right_knee_link.STL"/></geometry><material name="white"/></visual><collision><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/right_knee_link.STL"/></geometry></collision></link><joint name="right_knee_joint" type="revolute"><origin xyz="-0.078273 -0.0021489 -0.17734" rpy="0 0.1749 0"/><parent link="right_hip_yaw_link"/><child link="right_knee_link"/><axis xyz="0 1 0"/><limit lower="-0.087267" upper="2.8798" effort="139" velocity="20"/></joint><link name="right_ankle_pitch_link"><inertial><origin xyz="-0.007269 0 0.011137" rpy="0 0 0"/><mass value="0.074"/><inertia ixx="8.4E-06" ixy="0" ixz="-2.9E-06" iyy="1.89E-05" iyz="0" izz="1.26E-05"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/right_ankle_pitch_link.STL"/></geometry><material name="white"/></visual><collision><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/right_ankle_pitch_link.STL"/></geometry></collision></link><joint name="right_ankle_pitch_joint" type="revolute"><origin xyz="0 9.4445E-05 -0.30001" rpy="0 0 0"/><parent link="right_knee_link"/><child link="right_ankle_pitch_link"/><axis xyz="0 1 0"/><limit lower="-0.87267" upper="0.5236" effort="35" velocity="30"/></joint><link name="right_ankle_roll_link"><inertial><origin xyz="0.026505 0 -0.016425" rpy="0 0 0"/><mass value="0.608"/><inertia ixx="0.0002231" ixy="-2E-07" ixz="8.91E-05" iyy="0.0016161" iyz="1E-07" izz="0.0016667"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/right_ankle_roll_link.STL"/></geometry><material name="dark"/></visual><collision><origin xyz="-0.05 0.025 -0.03" rpy="0 0 0"/><geometry><sphere radius="0.005"/></geometry></collision><collision><origin xyz="-0.05 -0.025 -0.03" rpy="0 0 0"/><geometry><sphere radius="0.005"/></geometry></collision><collision><origin xyz="0.12 0.03 -0.03" rpy="0 0 0"/><geometry><sphere radius="0.005"/></geometry></collision><collision><origin xyz="0.12 -0.03 -0.03" rpy="0 0 0"/><geometry><sphere radius="0.005"/></geometry></collision></link><joint name="right_ankle_roll_joint" type="revolute"><origin xyz="0 0 -0.017558" rpy="0 0 0"/><parent link="right_ankle_pitch_link"/><child link="right_ankle_roll_link"/><axis xyz="1 0 0"/><limit lower="-0.2618" upper="0.2618" effort="35" velocity="30"/></joint><!-- Torso --><link name="waist_yaw_link"><inertial><origin xyz="0.003964 0 0.018769" rpy="0 0 0"/><mass value="0.244"/><inertia ixx="9.9587E-05" ixy="-1.833E-06" ixz="-1.2617E-05" iyy="0.00012411" iyz="-1.18E-07" izz="0.00015586"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/waist_yaw_link.STL"/></geometry><material name="white"/></visual></link><joint name="waist_yaw_joint" type="revolute"><origin xyz="0 0 0" rpy="0 0 0"/><parent link="pelvis"/><child link="waist_yaw_link"/><axis xyz="0 0 1"/><limit lower="-2.618" upper="2.618" effort="88" velocity="32"/></joint><link name="waist_roll_link"><inertial><origin xyz="0 -0.000236 0.010111" rpy="0 0 0"/><mass value="0.047"/><inertia ixx="7.515E-06" ixy="0" ixz="0" iyy="6.398E-06" iyz="9.9E-08" izz="3.988E-06"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/waist_roll_link.STL"/></geometry><material name="white"/></visual></link><joint name="waist_roll_joint" type="revolute"><origin xyz="-0.0039635 0 0.035" rpy="0 0 0"/><parent link="waist_yaw_link"/><child link="waist_roll_link"/><axis xyz="1 0 0"/><limit lower="-0.52" upper="0.52" effort="35" velocity="30"/></joint><link name="torso_link"><inertial><origin xyz="0.002601 0.000257 0.153719" rpy="0 0 0"/><mass value="8.562"/><inertia ixx="0.065674966" ixy="-8.597E-05" ixz="-0.001737252" iyy="0.053535188" iyz="8.6899E-05" izz="0.030808125"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/torso_link.STL"/></geometry><material name="white"/></visual><collision><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/torso_link.STL"/></geometry></collision></link><joint name="waist_pitch_joint" type="revolute"><origin xyz="0 0 0.019" rpy="0 0 0"/><parent link="waist_roll_link"/><child link="torso_link"/><axis xyz="0 1 0"/><limit lower="-0.52" upper="0.52" effort="35" velocity="30"/></joint><!-- LOGO --><joint name="logo_joint" type="fixed"><origin xyz="0.0039635 0 -0.054" rpy="0 0 0"/><parent link="torso_link"/><child link="logo_link"/></joint><link name="logo_link"><inertial><origin xyz="0 0 0" rpy="0 0 0"/><mass value="0.001"/><inertia ixx="1e-7" ixy="0" ixz="0" iyy="1e-7" iyz="0" izz="1e-7"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/logo_link.STL"/></geometry><material name="dark"/></visual><collision><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/logo_link.STL"/></geometry></collision></link><!-- Head --><link name="head_link"><inertial><origin xyz="0.005267 0.000299 0.449869" rpy="0 0 0"/><mass value="1.036"/><inertia ixx="0.004085051" ixy="-2.543E-06" ixz="-6.9455E-05" iyy="0.004185212" iyz="-3.726E-06" izz="0.001807911"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/head_link.STL"/></geometry><material name="dark"/></visual><collision><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/head_link.STL"/></geometry></collision></link><joint name="head_joint" type="fixed"><origin xyz="0.0039635 0 -0.054" rpy="0 0 0"/><parent link="torso_link"/><child link="head_link"/></joint><!-- Waist Support --><link name="waist_support_link"><inertial><origin xyz="0 0 0" rpy="0 0 0"/><mass value="0.001"/><inertia ixx="1e-7" ixy="0" ixz="0" iyy="1e-7" iyz="0" izz="1e-7"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/waist_support_link.STL"/></geometry><material name="white"/></visual><collision><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/waist_support_link.STL"/></geometry></collision></link><joint name="waist_support_joint" type="fixed"><origin xyz="0.0039635 0 -0.054" rpy="0 0 0"/><parent link="torso_link"/><child link="waist_support_link"/></joint><!-- IMU --><link name="imu_in_torso"></link><joint name="imu_in_torso_joint" type="fixed"><origin xyz="-0.03959 -0.00224 0.13792" rpy="0 0 0"/><parent link="torso_link"/><child link="imu_in_torso"/></joint><link name="imu_in_pelvis"></link><joint name="imu_in_pelvis_joint" type="fixed"><origin xyz="0.04525 0 -0.08339" rpy="0 0 0"/><parent link="pelvis"/><child link="imu_in_pelvis"/></joint><!-- d435 --><link name="d435_link"></link><joint name="d435_joint" type="fixed"><origin xyz="0.0576235 0.01753 0.41987" rpy="0 0.8307767239493009 0"/><parent link="torso_link"/><child link="d435_link"/></joint><!-- mid360 --><link name="mid360_link"></link><joint name="mid360_joint" type="fixed"><origin xyz="0.0002835 0.00003 0.40618" rpy="0 0.04014257279586953 0"/><parent link="torso_link"/><child link="mid360_link"/></joint><!-- Arm --><link name="left_shoulder_pitch_link"><inertial><origin xyz="0 0.035892 -0.011628" rpy="0 0 0"/><mass value="0.718"/><inertia ixx="0.0004291" ixy="-9.2E-06" ixz="6.4E-06" iyy="0.000453" iyz="2.26E-05" izz="0.000423"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/left_shoulder_pitch_link.STL"/></geometry><material name="white"/></visual><collision><origin xyz="0 0.04 -0.01" rpy="0 1.5707963267948966 0"/><geometry><cylinder radius="0.03" length="0.05"/></geometry></collision></link><joint name="left_shoulder_pitch_joint" type="revolute"><origin xyz="0.0039563 0.10022 0.23778" rpy="0.27931 5.4949E-05 -0.00019159"/><parent link="torso_link"/><child link="left_shoulder_pitch_link"/><axis xyz="0 1 0"/><limit lower="-3.0892" upper="2.6704" effort="25" velocity="37"/></joint><link name="left_shoulder_roll_link"><inertial><origin xyz="-0.000227 0.00727 -0.063243" rpy="0 0 0"/><mass value="0.643"/><inertia ixx="0.0006177" ixy="-1E-06" ixz="8.7E-06" iyy="0.0006912" iyz="-5.3E-06" izz="0.0003894"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/left_shoulder_roll_link.STL"/></geometry><material name="white"/></visual><collision><origin xyz="-0.004 0.006 -0.053" rpy="0 0 0"/><geometry><cylinder radius="0.03" length="0.03"/></geometry></collision></link><joint name="left_shoulder_roll_joint" type="revolute"><origin xyz="0 0.038 -0.013831" rpy="-0.27925 0 0"/><parent link="left_shoulder_pitch_link"/><child link="left_shoulder_roll_link"/><axis xyz="1 0 0"/><limit lower="-1.5882" upper="2.2515" effort="25" velocity="37"/></joint><link name="left_shoulder_yaw_link"><inertial><origin xyz="0.010773 -0.002949 -0.072009" rpy="0 0 0"/><mass value="0.734"/><inertia ixx="0.0009988" ixy="7.9E-06" ixz="0.0001412" iyy="0.0010605" iyz="-2.86E-05" izz="0.0004354"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/left_shoulder_yaw_link.STL"/></geometry><material name="white"/></visual><collision><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/left_shoulder_yaw_link.STL"/></geometry></collision></link><joint name="left_shoulder_yaw_joint" type="revolute"><origin xyz="0 0.00624 -0.1032" rpy="0 0 0"/><parent link="left_shoulder_roll_link"/><child link="left_shoulder_yaw_link"/><axis xyz="0 0 1"/><limit lower="-2.618" upper="2.618" effort="25" velocity="37"/></joint><link name="left_elbow_link"><inertial><origin xyz="0.064956 0.004454 -0.010062" rpy="0 0 0"/><mass value="0.6"/><inertia ixx="0.0002891" ixy="6.53E-05" ixz="1.72E-05" iyy="0.0004152" iyz="-5.6E-06" izz="0.0004197"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/left_elbow_link.STL"/></geometry><material name="white"/></visual><collision><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/left_elbow_link.STL"/></geometry></collision></link><joint name="left_elbow_joint" type="revolute"><origin xyz="0.015783 0 -0.080518" rpy="0 0 0"/><parent link="left_shoulder_yaw_link"/><child link="left_elbow_link"/><axis xyz="0 1 0"/><limit lower="-1.0472" upper="2.0944" effort="25" velocity="37"/></joint><joint name="left_wrist_roll_joint" type="revolute"><origin xyz="0.100 0.00188791 -0.010" rpy="0 0 0"/><axis xyz="1 0 0"/><parent link="left_elbow_link"/><child link="left_wrist_roll_link"/><limit effort="25" velocity="37" lower="-1.972222054" upper="1.972222054"/></joint><link name="left_wrist_roll_link"><inertial><origin xyz="0.01713944778 0.00053759094 0.00000048864" rpy="0 0 0"/><mass value="0.08544498"/><inertia ixx="0.00004821544023" ixy="-0.00000424511021" ixz="0.00000000510599" iyy="0.00003722899093" iyz="-0.00000000123525" izz="0.00005482106541"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/left_wrist_roll_link.STL"/></geometry><material name="white"/></visual><collision><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/left_wrist_roll_link.STL"/></geometry></collision></link><joint name="left_wrist_pitch_joint" type="revolute"><origin xyz="0.038 0 0" rpy="0 0 0"/><axis xyz="0 1 0"/><parent link="left_wrist_roll_link"/><child link="left_wrist_pitch_link"/><limit effort="5" velocity="22" lower="-1.614429558" upper="1.614429558"/></joint><link name="left_wrist_pitch_link"><inertial><origin xyz="0.02299989837 -0.00111685314 -0.00111658096" rpy="0 0 0"/><mass value="0.48404956"/><inertia ixx="0.00016579646273" ixy="-0.00001231206746" ixz="0.00001231699194" iyy="0.00042954057410" iyz="0.00000081417712" izz="0.00042953697654"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/left_wrist_pitch_link.STL"/></geometry><material name="white"/></visual><collision><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/left_wrist_pitch_link.STL"/></geometry></collision></link><joint name="left_wrist_yaw_joint" type="revolute"><origin xyz="0.046 0 0" rpy="0 0 0"/><axis xyz="0 0 1"/><parent link="left_wrist_pitch_link"/><child link="left_wrist_yaw_link"/><limit effort="5" velocity="22" lower="-1.614429558" upper="1.614429558"/></joint><link name="left_wrist_yaw_link"><inertial><origin xyz="0.02200381568 0.00049485096 0.00053861123" rpy="0 0 0"/><mass value="0.08457647"/><inertia ixx="0.00004929128828" ixy="-0.00000045735494" ixz="0.00000445867591" iyy="0.00005973338134" iyz="0.00000043217198" izz="0.00003928083826"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/left_wrist_yaw_link.STL"/></geometry><material name="white"/></visual><collision><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/left_wrist_yaw_link.STL"/></geometry></collision></link><joint name="left_hand_palm_joint" type="fixed"><origin xyz="0.0415 0.003 0" rpy="0 0 0"/><parent link="left_wrist_yaw_link"/><child link="left_rubber_hand"/></joint><link name="left_rubber_hand"><inertial><origin xyz="0.05361310808 -0.00295905240 0.00215413091" rpy="0 0 0"/><mass value="0.170"/><inertia ixx="0.00010099485234748" ixy="0.00003618590790516" ixz="-0.00000074301518642" iyy="0.00028135871571621" iyz="0.00000330189743286" izz="0.00021894770413514"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/left_rubber_hand.STL"/></geometry><material name="white"/></visual></link><link name="right_shoulder_pitch_link"><inertial><origin xyz="0 -0.035892 -0.011628" rpy="0 0 0"/><mass value="0.718"/><inertia ixx="0.0004291" ixy="9.2E-06" ixz="6.4E-06" iyy="0.000453" iyz="-2.26E-05" izz="0.000423"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/right_shoulder_pitch_link.STL"/></geometry><material name="white"/></visual><collision><origin xyz="0 -0.04 -0.01" rpy="0 1.5707963267948966 0"/><geometry><cylinder radius="0.03" length="0.05"/></geometry></collision></link><joint name="right_shoulder_pitch_joint" type="revolute"><origin xyz="0.0039563 -0.10021 0.23778" rpy="-0.27931 5.4949E-05 0.00019159"/><parent link="torso_link"/><child link="right_shoulder_pitch_link"/><axis xyz="0 1 0"/><limit lower="-3.0892" upper="2.6704" effort="25" velocity="37"/></joint><link name="right_shoulder_roll_link"><inertial><origin xyz="-0.000227 -0.00727 -0.063243" rpy="0 0 0"/><mass value="0.643"/><inertia ixx="0.0006177" ixy="1E-06" ixz="8.7E-06" iyy="0.0006912" iyz="5.3E-06" izz="0.0003894"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/right_shoulder_roll_link.STL"/></geometry><material name="white"/></visual><collision><origin xyz="-0.004 -0.006 -0.053" rpy="0 0 0"/><geometry><cylinder radius="0.03" length="0.03"/></geometry></collision></link><joint name="right_shoulder_roll_joint" type="revolute"><origin xyz="0 -0.038 -0.013831" rpy="0.27925 0 0"/><parent link="right_shoulder_pitch_link"/><child link="right_shoulder_roll_link"/><axis xyz="1 0 0"/><limit lower="-2.2515" upper="1.5882" effort="25" velocity="37"/></joint><link name="right_shoulder_yaw_link"><inertial><origin xyz="0.010773 0.002949 -0.072009" rpy="0 0 0"/><mass value="0.734"/><inertia ixx="0.0009988" ixy="-7.9E-06" ixz="0.0001412" iyy="0.0010605" iyz="2.86E-05" izz="0.0004354"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/right_shoulder_yaw_link.STL"/></geometry><material name="white"/></visual><collision><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/right_shoulder_yaw_link.STL"/></geometry></collision></link><joint name="right_shoulder_yaw_joint" type="revolute"><origin xyz="0 -0.00624 -0.1032" rpy="0 0 0"/><parent link="right_shoulder_roll_link"/><child link="right_shoulder_yaw_link"/><axis xyz="0 0 1"/><limit lower="-2.618" upper="2.618" effort="25" velocity="37"/></joint><link name="right_elbow_link"><inertial><origin xyz="0.064956 -0.004454 -0.010062" rpy="0 0 0"/><mass value="0.6"/><inertia ixx="0.0002891" ixy="-6.53E-05" ixz="1.72E-05" iyy="0.0004152" iyz="5.6E-06" izz="0.0004197"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/right_elbow_link.STL"/></geometry><material name="white"/></visual><collision><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/right_elbow_link.STL"/></geometry></collision></link><joint name="right_elbow_joint" type="revolute"><origin xyz="0.015783 0 -0.080518" rpy="0 0 0"/><parent link="right_shoulder_yaw_link"/><child link="right_elbow_link"/><axis xyz="0 1 0"/><limit lower="-1.0472" upper="2.0944" effort="25" velocity="37"/></joint><joint name="right_wrist_roll_joint" type="revolute"><origin xyz="0.100 -0.00188791 -0.010" rpy="0 0 0"/><axis xyz="1 0 0"/><parent link="right_elbow_link"/><child link="right_wrist_roll_link"/><limit effort="25" velocity="37" lower="-1.972222054" upper="1.972222054"/></joint><link name="right_wrist_roll_link"><inertial><origin xyz="0.01713944778 -0.00053759094 0.00000048864" rpy="0 0 0"/><mass value="0.08544498"/><inertia ixx="0.00004821544023" ixy="0.00000424511021" ixz="0.00000000510599" iyy="0.00003722899093" iyz="0.00000000123525" izz="0.00005482106541"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/right_wrist_roll_link.STL"/></geometry><material name="white"/></visual><collision><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/right_wrist_roll_link.STL"/></geometry></collision></link><joint name="right_wrist_pitch_joint" type="revolute"><origin xyz="0.038 0 0" rpy="0 0 0"/><axis xyz="0 1 0"/><parent link="right_wrist_roll_link"/><child link="right_wrist_pitch_link"/><limit effort="5" velocity="22" lower="-1.614429558" upper="1.614429558"/></joint><link name="right_wrist_pitch_link"><inertial><origin xyz="0.02299989837 0.00111685314 -0.00111658096" rpy="0 0 0"/><mass value="0.48404956"/><inertia ixx="0.00016579646273" ixy="0.00001231206746" ixz="0.00001231699194" iyy="0.00042954057410" iyz="-0.00000081417712" izz="0.00042953697654"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/right_wrist_pitch_link.STL"/></geometry><material name="white"/></visual><collision><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/right_wrist_pitch_link.STL"/></geometry></collision></link><joint name="right_wrist_yaw_joint" type="revolute"><origin xyz="0.046 0 0" rpy="0 0 0"/><axis xyz="0 0 1"/><parent link="right_wrist_pitch_link"/><child link="right_wrist_yaw_link"/><limit effort="5" velocity="22" lower="-1.614429558" upper="1.614429558"/></joint><link name="right_wrist_yaw_link"><inertial><origin xyz="0.02200381568 -0.00049485096 0.00053861123" rpy="0 0 0"/><mass value="0.08457647"/><inertia ixx="0.00004929128828" ixy="0.00000045735494" ixz="0.00000445867591" iyy="0.00005973338134" iyz="-0.00000043217198" izz="0.00003928083826"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/right_wrist_yaw_link.STL"/></geometry><material name="white"/></visual><collision><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/right_wrist_yaw_link.STL"/></geometry></collision></link><joint name="right_hand_palm_joint" type="fixed"><origin xyz="0.0415 -0.003 0" rpy="0 0 0"/><parent link="right_wrist_yaw_link"/><child link="right_rubber_hand"/></joint><link name="right_rubber_hand"><inertial><origin xyz="0.05361310808 0.00295905240 0.00215413091" rpy="0 0 0"/><mass value="0.170"/><inertia ixx="0.00010099485234748" ixy="-0.00003618590790516" ixz="-0.00000074301518642" iyy="0.00028135871571621" iyz="-0.00000330189743286" izz="0.00021894770413514"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://g1_description/meshes/right_rubber_hand.STL"/></geometry><material name="white"/></visual></link><gazebo reference="pelvis"><material>Gazebo/DarkGrey</material></gazebo><gazebo reference="left_hip_pitch_link"><material>Gazebo/DarkGrey</material></gazebo><gazebo reference="left_ankle_roll_link"><material>Gazebo/DarkGrey</material></gazebo><gazebo reference="right_hip_pitch_link"><material>Gazebo/DarkGrey</material></gazebo><gazebo reference="right_ankle_roll_link"><material>Gazebo/DarkGrey</material></gazebo><gazebo reference="logo_link"><material>Gazebo/DarkGrey</material></gazebo><gazebo reference="head_link"><material>Gazebo/DarkGrey</material></gazebo><gazebo reference="pelvis_contour_link"><material>Gazebo/Grey</material></gazebo><gazebo reference="left_hip_roll_link"><material>Gazebo/Grey</material></gazebo><gazebo reference="left_hip_yaw_link"><material>Gazebo/Grey</material></gazebo><gazebo reference="left_knee_link"><material>Gazebo/Grey</material></gazebo><gazebo reference="left_ankle_pitch_link"><material>Gazebo/Grey</material></gazebo><gazebo reference="right_hip_roll_link"><material>Gazebo/Grey</material></gazebo><gazebo reference="right_hip_yaw_link"><material>Gazebo/Grey</material></gazebo><gazebo reference="right_knee_link"><material>Gazebo/Grey</material></gazebo><gazebo reference="right_ankle_pitch_link"><material>Gazebo/Grey</material></gazebo><gazebo reference="waist_yaw_link"><material>Gazebo/Grey</material></gazebo><gazebo reference="waist_roll_link"><material>Gazebo/Grey</material></gazebo><gazebo reference="torso_link"><material>Gazebo/Grey</material></gazebo><gazebo reference="waist_support_link"><material>Gazebo/Grey</material></gazebo><gazebo reference="left_shoulder_pitch_link"><material>Gazebo/Grey</material></gazebo><gazebo reference="left_shoulder_roll_link"><material>Gazebo/Grey</material></gazebo><gazebo reference="left_shoulder_yaw_link"><material>Gazebo/Grey</material></gazebo><gazebo reference="left_elbow_link"><material>Gazebo/Grey</material></gazebo><gazebo reference="left_wrist_roll_link"><material>Gazebo/Grey</material></gazebo><gazebo reference="left_wrist_pitch_link"><material>Gazebo/Grey</material></gazebo><gazebo reference="left_wrist_yaw_link"><material>Gazebo/Grey</material></gazebo><gazebo reference="left_rubber_hand"><material>Gazebo/Grey</material></gazebo><gazebo reference="right_shoulder_pitch_link"><material>Gazebo/Grey</material></gazebo><gazebo reference="right_shoulder_roll_link"><material>Gazebo/Grey</material></gazebo><gazebo reference="right_shoulder_yaw_link"><material>Gazebo/Grey</material></gazebo><gazebo reference="right_elbow_link"><material>Gazebo/Grey</material></gazebo><gazebo reference="right_wrist_roll_link"><material>Gazebo/Grey</material></gazebo><gazebo reference="right_wrist_pitch_link"><material>Gazebo/Grey</material></gazebo><gazebo reference="right_wrist_yaw_link"><material>Gazebo/Grey</material></gazebo><gazebo reference="right_rubber_hand"><material>Gazebo/Grey</material></gazebo><!-- Transmissions for ros_control --><!-- Leg Transmissions --><transmission name="left_hip_pitch_trans"><type>transmission_interface/SimpleTransmission</type><joint name="left_hip_pitch_joint"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></joint><actuator name="left_hip_pitch_motor"><mechanicalReduction>1</mechanicalReduction><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></actuator></transmission><transmission name="left_hip_roll_trans"><type>transmission_interface/SimpleTransmission</type><joint name="left_hip_roll_joint"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></joint><actuator name="left_hip_roll_motor"><mechanicalReduction>1</mechanicalReduction><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></actuator></transmission><transmission name="left_hip_yaw_trans"><type>transmission_interface/SimpleTransmission</type><joint name="left_hip_yaw_joint"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></joint><actuator name="left_hip_yaw_motor"><mechanicalReduction>1</mechanicalReduction><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></actuator></transmission><transmission name="left_knee_trans"><type>transmission_interface/SimpleTransmission</type><joint name="left_knee_joint"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></joint><actuator name="left_knee_motor"><mechanicalReduction>1</mechanicalReduction><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></actuator></transmission><transmission name="left_ankle_pitch_trans"><type>transmission_interface/SimpleTransmission</type><joint name="left_ankle_pitch_joint"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></joint><actuator name="left_ankle_pitch_motor"><mechanicalReduction>1</mechanicalReduction><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></actuator></transmission><transmission name="left_ankle_roll_trans"><type>transmission_interface/SimpleTransmission</type><joint name="left_ankle_roll_joint"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></joint><actuator name="left_ankle_roll_motor"><mechanicalReduction>1</mechanicalReduction><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></actuator></transmission><transmission name="right_hip_pitch_trans"><type>transmission_interface/SimpleTransmission</type><joint name="right_hip_pitch_joint"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></joint><actuator name="right_hip_pitch_motor"><mechanicalReduction>1</mechanicalReduction><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></actuator></transmission><transmission name="right_hip_roll_trans"><type>transmission_interface/SimpleTransmission</type><joint name="right_hip_roll_joint"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></joint><actuator name="right_hip_roll_motor"><mechanicalReduction>1</mechanicalReduction><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></actuator></transmission><transmission name="right_hip_yaw_trans"><type>transmission_interface/SimpleTransmission</type><joint name="right_hip_yaw_joint"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></joint><actuator name="right_hip_yaw_motor"><mechanicalReduction>1</mechanicalReduction><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></actuator></transmission><transmission name="right_knee_trans"><type>transmission_interface/SimpleTransmission</type><joint name="right_knee_joint"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></joint><actuator name="right_knee_motor"><mechanicalReduction>1</mechanicalReduction><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></actuator></transmission><transmission name="right_ankle_pitch_trans"><type>transmission_interface/SimpleTransmission</type><joint name="right_ankle_pitch_joint"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></joint><actuator name="right_ankle_pitch_motor"><mechanicalReduction>1</mechanicalReduction><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></actuator></transmission><transmission name="right_ankle_roll_trans"><type>transmission_interface/SimpleTransmission</type><joint name="right_ankle_roll_joint"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></joint><actuator name="right_ankle_roll_motor"><mechanicalReduction>1</mechanicalReduction><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></actuator></transmission><!-- Torso Transmissions --><transmission name="waist_yaw_trans"><type>transmission_interface/SimpleTransmission</type><joint name="waist_yaw_joint"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></joint><actuator name="waist_yaw_motor"><mechanicalReduction>1</mechanicalReduction><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></actuator></transmission><transmission name="waist_roll_trans"><type>transmission_interface/SimpleTransmission</type><joint name="waist_roll_joint"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></joint><actuator name="waist_roll_motor"><mechanicalReduction>1</mechanicalReduction><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></actuator></transmission><transmission name="waist_pitch_trans"><type>transmission_interface/SimpleTransmission</type><joint name="waist_pitch_joint"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></joint><actuator name="waist_pitch_motor"><mechanicalReduction>1</mechanicalReduction><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></actuator></transmission><!-- Arm Transmissions --><transmission name="left_shoulder_pitch_trans"><type>transmission_interface/SimpleTransmission</type><joint name="left_shoulder_pitch_joint"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></joint><actuator name="left_shoulder_pitch_motor"><mechanicalReduction>1</mechanicalReduction><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></actuator></transmission><transmission name="left_shoulder_roll_trans"><type>transmission_interface/SimpleTransmission</type><joint name="left_shoulder_roll_joint"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></joint><actuator name="left_shoulder_roll_motor"><mechanicalReduction>1</mechanicalReduction><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></actuator></transmission><transmission name="left_shoulder_yaw_trans"><type>transmission_interface/SimpleTransmission</type><joint name="left_shoulder_yaw_joint"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></joint><actuator name="left_shoulder_yaw_motor"><mechanicalReduction>1</mechanicalReduction><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></actuator></transmission><transmission name="left_elbow_trans"><type>transmission_interface/SimpleTransmission</type><joint name="left_elbow_joint"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></joint><actuator name="left_elbow_motor"><mechanicalReduction>1</mechanicalReduction><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></actuator></transmission><transmission name="left_wrist_roll_trans"><type>transmission_interface/SimpleTransmission</type><joint name="left_wrist_roll_joint"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></joint><actuator name="left_wrist_roll_motor"><mechanicalReduction>1</mechanicalReduction><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></actuator></transmission><transmission name="left_wrist_pitch_trans"><type>transmission_interface/SimpleTransmission</type><joint name="left_wrist_pitch_joint"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></joint><actuator name="left_wrist_pitch_motor"><mechanicalReduction>1</mechanicalReduction><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></actuator></transmission><transmission name="left_wrist_yaw_trans"><type>transmission_interface/SimpleTransmission</type><joint name="left_wrist_yaw_joint"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></joint><actuator name="left_wrist_yaw_motor"><mechanicalReduction>1</mechanicalReduction><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></actuator></transmission><transmission name="right_shoulder_pitch_trans"><type>transmission_interface/SimpleTransmission</type><joint name="right_shoulder_pitch_joint"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></joint><actuator name="right_shoulder_pitch_motor"><mechanicalReduction>1</mechanicalReduction><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></actuator></transmission><transmission name="right_shoulder_roll_trans"><type>transmission_interface/SimpleTransmission</type><joint name="right_shoulder_roll_joint"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></joint><actuator name="right_shoulder_roll_motor"><mechanicalReduction>1</mechanicalReduction><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></actuator></transmission><transmission name="right_shoulder_yaw_trans"><type>transmission_interface/SimpleTransmission</type><joint name="right_shoulder_yaw_joint"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></joint><actuator name="right_shoulder_yaw_motor"><mechanicalReduction>1</mechanicalReduction><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></actuator></transmission><transmission name="right_elbow_trans"><type>transmission_interface/SimpleTransmission</type><joint name="right_elbow_joint"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></joint><actuator name="right_elbow_motor"><mechanicalReduction>1</mechanicalReduction><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></actuator></transmission><transmission name="right_wrist_roll_trans"><type>transmission_interface/SimpleTransmission</type><joint name="right_wrist_roll_joint"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></joint><actuator name="right_wrist_roll_motor"><mechanicalReduction>1</mechanicalReduction><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></actuator></transmission><transmission name="right_wrist_pitch_trans"><type>transmission_interface/SimpleTransmission</type><joint name="right_wrist_pitch_joint"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></joint><actuator name="right_wrist_pitch_motor"><mechanicalReduction>1</mechanicalReduction><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></actuator></transmission><transmission name="right_wrist_yaw_trans"><type>transmission_interface/SimpleTransmission</type><joint name="right_wrist_yaw_joint"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></joint><actuator name="right_wrist_yaw_motor"><mechanicalReduction>1</mechanicalReduction><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></actuator></transmission><!-- Gazebo ROS Control Plugin --><gazebo><plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so"><robotNamespace>/</robotNamespace><robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType><legacyModeNS>true</legacyModeNS></plugin></gazebo></robot>

主要文件

controllers.yaml

# ros_control configuration for g1 robot
# This file defines all the controllers for the robot joints
# NOTE: robotNamespace is set to "/" in launch file, so configs go at root level# Joint State Controller - publishes /joint_states from Gazebo
joint_state_controller:type: joint_state_controller/JointStateControllerpublish_rate: 50# Effort Controllers for all joints
# Legs
left_hip_pitch_controller:type: effort_controllers/JointEffortControllerjoint: left_hip_pitch_jointpid: {p: 100.0, i: 1.0, d: 10.0}left_hip_roll_controller:type: effort_controllers/JointEffortControllerjoint: left_hip_roll_jointpid: {p: 100.0, i: 1.0, d: 10.0}left_hip_yaw_controller:type: effort_controllers/JointEffortControllerjoint: left_hip_yaw_jointpid: {p: 100.0, i: 1.0, d: 10.0}left_knee_controller:type: effort_controllers/JointEffortControllerjoint: left_knee_jointpid: {p: 150.0, i: 1.0, d: 15.0}left_ankle_pitch_controller:type: effort_controllers/JointEffortControllerjoint: left_ankle_pitch_jointpid: {p: 50.0, i: 0.5, d: 5.0}left_ankle_roll_controller:type: effort_controllers/JointEffortControllerjoint: left_ankle_roll_jointpid: {p: 50.0, i: 0.5, d: 5.0}right_hip_pitch_controller:type: effort_controllers/JointEffortControllerjoint: right_hip_pitch_jointpid: {p: 100.0, i: 1.0, d: 10.0}right_hip_roll_controller:type: effort_controllers/JointEffortControllerjoint: right_hip_roll_jointpid: {p: 100.0, i: 1.0, d: 10.0}right_hip_yaw_controller:type: effort_controllers/JointEffortControllerjoint: right_hip_yaw_jointpid: {p: 100.0, i: 1.0, d: 10.0}right_knee_controller:type: effort_controllers/JointEffortControllerjoint: right_knee_jointpid: {p: 150.0, i: 1.0, d: 15.0}right_ankle_pitch_controller:type: effort_controllers/JointEffortControllerjoint: right_ankle_pitch_jointpid: {p: 50.0, i: 0.5, d: 5.0}right_ankle_roll_controller:type: effort_controllers/JointEffortControllerjoint: right_ankle_roll_jointpid: {p: 50.0, i: 0.5, d: 5.0}# Torso
waist_yaw_controller:type: effort_controllers/JointEffortControllerjoint: waist_yaw_jointpid: {p: 80.0, i: 0.8, d: 8.0}waist_roll_controller:type: effort_controllers/JointEffortControllerjoint: waist_roll_jointpid: {p: 50.0, i: 0.5, d: 5.0}waist_pitch_controller:type: effort_controllers/JointEffortControllerjoint: waist_pitch_jointpid: {p: 50.0, i: 0.5, d: 5.0}# Left Arm
left_shoulder_pitch_controller:type: effort_controllers/JointEffortControllerjoint: left_shoulder_pitch_jointpid: {p: 50.0, i: 0.5, d: 5.0}left_shoulder_roll_controller:type: effort_controllers/JointEffortControllerjoint: left_shoulder_roll_jointpid: {p: 50.0, i: 0.5, d: 5.0}left_shoulder_yaw_controller:type: effort_controllers/JointEffortControllerjoint: left_shoulder_yaw_jointpid: {p: 50.0, i: 0.5, d: 5.0}left_elbow_controller:type: effort_controllers/JointEffortControllerjoint: left_elbow_jointpid: {p: 50.0, i: 0.5, d: 5.0}left_wrist_roll_controller:type: effort_controllers/JointEffortControllerjoint: left_wrist_roll_jointpid: {p: 25.0, i: 0.25, d: 2.5}left_wrist_pitch_controller:type: effort_controllers/JointEffortControllerjoint: left_wrist_pitch_jointpid: {p: 25.0, i: 0.25, d: 2.5}left_wrist_yaw_controller:type: effort_controllers/JointEffortControllerjoint: left_wrist_yaw_jointpid: {p: 25.0, i: 0.25, d: 2.5}# Right Arm
right_shoulder_pitch_controller:type: effort_controllers/JointEffortControllerjoint: right_shoulder_pitch_jointpid: {p: 50.0, i: 0.5, d: 5.0}right_shoulder_roll_controller:type: effort_controllers/JointEffortControllerjoint: right_shoulder_roll_jointpid: {p: 50.0, i: 0.5, d: 5.0}right_shoulder_yaw_controller:type: effort_controllers/JointEffortControllerjoint: right_shoulder_yaw_jointpid: {p: 50.0, i: 0.5, d: 5.0}right_elbow_controller:type: effort_controllers/JointEffortControllerjoint: right_elbow_jointpid: {p: 50.0, i: 0.5, d: 5.0}right_wrist_roll_controller:type: effort_controllers/JointEffortControllerjoint: right_wrist_roll_jointpid: {p: 25.0, i: 0.25, d: 2.5}right_wrist_pitch_controller:type: effort_controllers/JointEffortControllerjoint: right_wrist_pitch_jointpid: {p: 25.0, i: 0.25, d: 2.5}right_wrist_yaw_controller:type: effort_controllers/JointEffortControllerjoint: right_wrist_yaw_jointpid: {p: 25.0, i: 0.25, d: 2.5}

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"/><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 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" /><!-- 加载ros_control控制器配置 --><rosparam file="$(find g1_description)/config/controllers.yaml" command="load"/><!-- ros_control控制管理器 --><node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"output="screen" args="joint_state_controllerleft_hip_pitch_controllerleft_hip_roll_controllerleft_hip_yaw_controllerleft_knee_controllerleft_ankle_pitch_controllerleft_ankle_roll_controllerright_hip_pitch_controllerright_hip_roll_controllerright_hip_yaw_controllerright_knee_controllerright_ankle_pitch_controllerright_ankle_roll_controllerwaist_yaw_controllerwaist_roll_controllerwaist_pitch_controllerleft_shoulder_pitch_controllerleft_shoulder_roll_controllerleft_shoulder_yaw_controllerleft_elbow_controllerleft_wrist_roll_controllerleft_wrist_pitch_controllerleft_wrist_yaw_controllerright_shoulder_pitch_controllerright_shoulder_roll_controllerright_shoulder_yaw_controllerright_elbow_controllerright_wrist_roll_controllerright_wrist_pitch_controllerright_wrist_yaw_controller" /><!-- TF广播器 - 发布map->base_link的动态变换 --><node name="tf_broadcaster" pkg="g1_description" type="tf_broadcaster.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>

tf_broadcaster.py

#!/usr/bin/env python3
"""
TF Broadcaster for G1 Robot
This script publishes the base_link to map transform based on Gazebo's pelvis position.
Joint state publishing is now handled by ros_control's joint_state_controller.
"""import rospy
import tf
import threading
import numpy as np
from gazebo_msgs.msg import LinkStates
from scipy.spatial.transform import Rotationclass TFBroadcaster:def __init__(self):rospy.init_node('g1_tf_broadcaster')# Subscribe to Gazebo link statesself.link_states_sub = rospy.Subscriber('/gazebo/link_states', LinkStates, self.link_states_callback, queue_size=1)# TF broadcasterself.tf_broadcaster = tf.TransformBroadcaster()# Throttling parameters - more aggressive throttlingself.last_tf_time = rospy.Time(0)self.tf_min_interval = rospy.Duration(0.1)  # 100ms minimum between publishesself.last_pelvis_pose = Noneself.tf_pos_thresh = 0.01   # 1cm thresholdself.tf_rot_thresh = 0.02   # ~1.1 deg thresholdself.lock = threading.Lock()rospy.loginfo("TF Broadcaster initialized")rospy.loginfo("Publishing map -> base_link transform from Gazebo pelvis position")rospy.loginfo("Joint state publishing is handled by ros_control/joint_state_controller")def get_link_index(self, link_name, msg):"""Get link index in LinkStates message"""full_name = f'g1_robot::{link_name}'try:return msg.name.index(full_name)except ValueError:return -1def link_states_callback(self, msg):"""Handle LinkStates message and publish TF transform"""with self.lock:self.publish_dynamic_tf(msg)def publish_dynamic_tf(self, msg):"""Publish dynamic TF from pelvis to base_link"""pelvis_idx = self.get_link_index('pelvis', msg)if pelvis_idx < 0:rospy.logwarn_once("Could not find pelvis in link states")returnpelvis_pose = msg.pose[pelvis_idx]# Throttling logic - only publish if significant changenow = rospy.Time.now()time_since_last = now - self.last_tf_timeif self.last_pelvis_pose is not None:# Only check threshold if enough time has passedif time_since_last < self.tf_min_interval:returnpos_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()# Skip if changes are too smallif pos_diff < self.tf_pos_thresh and rot_diff_angle < self.tf_rot_thresh:returnself.last_tf_time = nowself.last_pelvis_pose = pelvis_posetranslation = (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)# Publish map -> base_link transformself.tf_broadcaster.sendTransform(translation=translation,rotation=rotation_q,time=now,child='base_link',parent='map')if __name__ == '__main__':try:broadcaster = TFBroadcaster()rospy.spin()except rospy.ROSInterruptException:pass

CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(g1_description)find_package(catkin REQUIRED COMPONENTSurdfxacro
)catkin_package()# 安装路径设置，确保launch和meshes能被找到
install(DIRECTORY launch meshes urdfDESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

package.xml

<?xml version="1.0"?>
<package format="2"><name>g1_description</name><version>0.0.1</version><description>The g1_description package</description><maintainer email="zyl_0606@163.com">User</maintainer><license>TODO</license><buildtool_depend>catkin</buildtool_depend><depend>roscpp</depend><depend>rospy</depend><depend>std_msgs</depend><depend>urdf</depend><depend>xacro</depend><depend>tf</depend><depend>sensor_msgs</depend><depend>geometry_msgs</depend><exec_depend>gazebo_ros</exec_depend><exec_depend>joint_state_publisher</exec_depend><exec_depend>robot_state_publisher</exec_depend><exec_depend>rviz</exec_depend><exec_depend>ros_control</exec_depend><exec_depend>ros_controllers</exec_depend><exec_depend>controller_manager</exec_depend><exec_depend>effort_controllers</exec_depend><exec_depend>joint_state_controller</exec_depend><exec_depend>gazebo_ros_control</exec_depend><export><gazebo_ros gazebo_model_path="${prefix}/.."/></export>
</package>

运行步骤

cd ~/g1_test2_ws

catkin_make

source ./devel/setup.bash

roslaunch g1_description display_and_gazebo.launch