ROS1 noetic 中将 Unitree G1 基于 Gazebo/RViz 关节联动【基于 ros_control】
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/JointStateController
publish_rate: 50
# Effort Controllers for all joints
# Legs
left_hip_pitch_controller:
type: effort_controllers/JointEffortController
joint: left_hip_pitch_joint
pid: {p: 100.0, i: 1.0, d: 10.0}
left_hip_roll_controller:
type: effort_controllers/JointEffortController
joint: left_hip_roll_joint
pid: {p: 100.0, i: 1.0, d: 10.0}
left_hip_yaw_controller:
type: effort_controllers/JointEffortController
joint: left_hip_yaw_joint
pid: {p: 100.0, i: 1.0, d: 10.0}
left_knee_controller:
type: effort_controllers/JointEffortController
joint: left_knee_joint
pid: {p: 150.0, i: 1.0, d: 15.0}
left_ankle_pitch_controller:
type: effort_controllers/JointEffortController
joint: left_ankle_pitch_joint
pid: {p: 50.0, i: 0.5, d: 5.0}
left_ankle_roll_controller:
type: effort_controllers/JointEffortController
joint: left_ankle_roll_joint
pid: {p: 50.0, i: 0.5, d: 5.0}
right_hip_pitch_controller:
type: effort_controllers/JointEffortController
joint: right_hip_pitch_joint
pid: {p: 100.0, i: 1.0, d: 10.0}
right_hip_roll_controller:
type: effort_controllers/JointEffortController
joint: right_hip_roll_joint
pid: {p: 100.0, i: 1.0, d: 10.0}
right_hip_yaw_controller:
type: effort_controllers/JointEffortController
joint: right_hip_yaw_joint
pid: {p: 100.0, i: 1.0, d: 10.0}
right_knee_controller:
type: effort_controllers/JointEffortController
joint: right_knee_joint
pid: {p: 150.0, i: 1.0, d: 15.0}
right_ankle_pitch_controller:
type: effort_controllers/JointEffortController
joint: right_ankle_pitch_joint
pid: {p: 50.0, i: 0.5, d: 5.0}
right_ankle_roll_controller:
type: effort_controllers/JointEffortController
joint: right_ankle_roll_joint
pid: {p: 50.0, i: 0.5, d: 5.0}
# Torso
waist_yaw_controller:
type: effort_controllers/JointEffortController
joint: waist_yaw_joint
pid: {p: 80.0, i: 0.8, d: 8.0}
waist_roll_controller:
type: effort_controllers/JointEffortController
joint: waist_roll_joint
pid: {p: 50.0, i: 0.5, d: 5.0}
waist_pitch_controller:
type: effort_controllers/JointEffortController
joint: waist_pitch_joint
pid: {p: 50.0, i: 0.5, d: 5.0}
# Left Arm
left_shoulder_pitch_controller:
type: effort_controllers/JointEffortController
joint: left_shoulder_pitch_joint
pid: {p: 50.0, i: 0.5, d: 5.0}
left_shoulder_roll_controller:
type: effort_controllers/JointEffortController
joint: left_shoulder_roll_joint
pid: {p: 50.0, i: 0.5, d: 5.0}
left_shoulder_yaw_controller:
type: effort_controllers/JointEffortController
joint: left_shoulder_yaw_joint
pid: {p: 50.0, i: 0.5, d: 5.0}
left_elbow_controller:
type: effort_controllers/JointEffortController
joint: left_elbow_joint
pid: {p: 50.0, i: 0.5, d: 5.0}
left_wrist_roll_controller:
type: effort_controllers/JointEffortController
joint: left_wrist_roll_joint
pid: {p: 25.0, i: 0.25, d: 2.5}
left_wrist_pitch_controller:
type: effort_controllers/JointEffortController
joint: left_wrist_pitch_joint
pid: {p: 25.0, i: 0.25, d: 2.5}
left_wrist_yaw_controller:
type: effort_controllers/JointEffortController
joint: left_wrist_yaw_joint
pid: {p: 25.0, i: 0.25, d: 2.5}
# Right Arm
right_shoulder_pitch_controller:
type: effort_controllers/JointEffortController
joint: right_shoulder_pitch_joint
pid: {p: 50.0, i: 0.5, d: 5.0}
right_shoulder_roll_controller:
type: effort_controllers/JointEffortController
joint: right_shoulder_roll_joint
pid: {p: 50.0, i: 0.5, d: 5.0}
right_shoulder_yaw_controller:
type: effort_controllers/JointEffortController
joint: right_shoulder_yaw_joint
pid: {p: 50.0, i: 0.5, d: 5.0}
right_elbow_controller:
type: effort_controllers/JointEffortController
joint: right_elbow_joint
pid: {p: 50.0, i: 0.5, d: 5.0}
right_wrist_roll_controller:
type: effort_controllers/JointEffortController
joint: right_wrist_roll_joint
pid: {p: 25.0, i: 0.25, d: 2.5}
right_wrist_pitch_controller:
type: effort_controllers/JointEffortController
joint: right_wrist_pitch_joint
pid: {p: 25.0, i: 0.25, d: 2.5}
right_wrist_yaw_controller:
type: effort_controllers/JointEffortController
joint: right_wrist_yaw_joint
pid: {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_controller
left_hip_pitch_controller
left_hip_roll_controller
left_hip_yaw_controller
left_knee_controller
left_ankle_pitch_controller
left_ankle_roll_controller
right_hip_pitch_controller
right_hip_roll_controller
right_hip_yaw_controller
right_knee_controller
right_ankle_pitch_controller
right_ankle_roll_controller
waist_yaw_controller
waist_roll_controller
waist_pitch_controller
left_shoulder_pitch_controller
left_shoulder_roll_controller
left_shoulder_yaw_controller
left_elbow_controller
left_wrist_roll_controller
left_wrist_pitch_controller
left_wrist_yaw_controller
right_shoulder_pitch_controller
right_shoulder_roll_controller
right_shoulder_yaw_controller
right_elbow_controller
right_wrist_roll_controller
right_wrist_pitch_controller
right_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 Rotation
class TFBroadcaster:
def __init__(self):
rospy.init_node('g1_tf_broadcaster')
# Subscribe to Gazebo link states
self.link_states_sub = rospy.Subscriber(
'/gazebo/link_states',
LinkStates,
self.link_states_callback,
queue_size=1
)
# TF broadcaster
self.tf_broadcaster = tf.TransformBroadcaster()
# Throttling parameters - more aggressive throttling
self.last_tf_time = rospy.Time(0)
self.tf_min_interval = rospy.Duration(0.1) # 100ms minimum between publishes
self.last_pelvis_pose = None
self.tf_pos_thresh = 0.01 # 1cm threshold
self.tf_rot_thresh = 0.02 # ~1.1 deg threshold
self.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 -1
def 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")
return
pelvis_pose = msg.pose[pelvis_idx]
# Throttling logic - only publish if significant change
now = rospy.Time.now()
time_since_last = now - self.last_tf_time
if self.last_pelvis_pose is not None:
# Only check threshold if enough time has passed
if time_since_last < self.tf_min_interval:
return
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()
# Skip if changes are too small
if pos_diff < self.tf_pos_thresh and rot_diff_angle < self.tf_rot_thresh:
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
)
# Publish map -> base_link transform
self.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 COMPONENTS
urdf
xacro
)
catkin_package()
# 安装路径设置，确保launch和meshes能被找到
install(DIRECTORY launch meshes urdf
DESTINATION ${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
浙公网安备 33010602011771号