EmbodiedAI
/
quadruped_ros2_control
mirror of https://github.com/legubiao/quadruped_ros2_control.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

add gazebo simulation for go2

This commit is contained in:
Zhenbiao Huang 2024-10-10 10:23:54 +08:00
parent 6385d8ed58
commit 87b9759ddf
8 changed files with 854 additions and 611 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
descriptions/README.md
View File

@ -3,19 +3,20 @@
This folder contains the URDF and SRDF files for the quadruped robot. This folder contains the URDF and SRDF files for the quadruped robot.
* Unitree * Unitree
* [Go1](unitree/go1_description/) * [Go1](unitree/go1_description/)
* [Go2](unitree/go2_description/) * [Go2](unitree/go2_description/)
* [A1](unitree/a1_description/) * [A1](unitree/a1_description/)
* [Aliengo](unitree/aliengo_description/) * [Aliengo](unitree/aliengo_description/)
* [B2](unitree/b2_description/) * [B2](unitree/b2_description/)
* Xiaomi * Xiaomi
* [Cyberdog](xiaomi/cyberdog_description/) * [Cyberdog](xiaomi/cyberdog_description/)
## Steps to transfer urdf to Mujoco model ## Steps to transfer urdf to Mujoco model
* Install [Mujoco](https://github.com/google-deepmind/mujoco) * Install [Mujoco](https://github.com/google-deepmind/mujoco)
* Transfer the mesh files to mujoco supported format, like stl. * Transfer the mesh files to mujoco supported format, like stl.
* Adjust the urdf tile to match the mesh file. Transfer the mesh file from .dae to .stl may change the scale size of the mesh file. * Adjust the urdf tile to match the mesh file. Transfer the mesh file from .dae to .stl may change the scale size of the
mesh file.
* use `xacro` to generate the urdf file. * use `xacro` to generate the urdf file.
``` ```
xacro robot.xacro > ../urdf/robot.urdf xacro robot.xacro > ../urdf/robot.urdf
@ -24,3 +25,19 @@ This folder contains the URDF and SRDF files for the quadruped robot.
``` ```
compile robot.urdf robot.xml compile robot.urdf robot.xml
``` ```
## Dependencies for Gazebo Simulation
Gazebo Simulation only tested on ROS2 Jazzy. I didn't add support for ROS2 Humble because the package name is different.
* Gazebo Harmonic
```bash
sudo apt-get install ros-jazzy-ros-gz
```
* Ros2-Control for Gazebo
```bash
sudo apt-get install ros-jazzy-gz-ros2-control
```
* Legged PD Controller
```bash
cd ~/ros2_ws
colcon build --packages-up-to leg_pd_controller
```

416
descriptions/unitree/a1_description/xacro/leg.xacro
View File

@ -2,225 +2,229 @@
<robot xmlns:xacro="http://www.ros.org/wiki/xacro"> <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find a1_description)/xacro/transmission.xacro"/> <xacro:include filename="$(find a1_description)/xacro/transmission.xacro"/>
<xacro:macro name="leg" params="name mirror mirror_dae front_hind front_hind_dae *origin"> <xacro:macro name="leg" params="name mirror mirror_dae front_hind front_hind_dae *origin">
<joint name="${name}_hip_joint" type="revolute"> <joint name="${name}_hip_joint" type="revolute">
<xacro:insert_block name="origin"/> <xacro:insert_block name="origin"/>
<parent link="trunk"/> <parent link="trunk"/>
<child link="${name}_hip"/> <child link="${name}_hip"/>
<axis xyz="1 0 0"/> <axis xyz="1 0 0"/>
<dynamics damping="${damping}" friction="${friction}"/> <dynamics damping="${damping}" friction="${friction}"/>
<xacro:if value="${(mirror_dae == True)}"> <xacro:if value="${(mirror_dae == True)}">
<limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${hip_min*PI/180.0}" upper="${hip_max*PI/180.0}"/> <limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${hip_min*PI/180.0}"
</xacro:if> upper="${hip_max*PI/180.0}"/>
<xacro:if value="${(mirror_dae == False)}"> </xacro:if>
<limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${-hip_max*PI/180.0}" upper="${-hip_min*PI/180.0}"/> <xacro:if value="${(mirror_dae == False)}">
</xacro:if> <limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${-hip_max*PI/180.0}"
</joint> upper="${-hip_min*PI/180.0}"/>
</xacro:if>
</joint>
<link name="${name}_hip"> <link name="${name}_hip">
<visual> <visual>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}"> <xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>
</xacro:if> </xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}"> <xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
<origin rpy="${PI} 0 0" xyz="0 0 0"/> <origin rpy="${PI} 0 0" xyz="0 0 0"/>
</xacro:if> </xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}"> <xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
<origin rpy="0 ${PI} 0" xyz="0 0 0"/> <origin rpy="0 ${PI} 0" xyz="0 0 0"/>
</xacro:if> </xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}"> <xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
<origin rpy="${PI} ${PI} 0" xyz="0 0 0"/> <origin rpy="${PI} ${PI} 0" xyz="0 0 0"/>
</xacro:if> </xacro:if>
<geometry> <geometry>
<mesh filename="file://$(find a1_description)/meshes/hip.dae" scale="1 1 1"/> <mesh filename="file://$(find a1_description)/meshes/hip.dae" scale="1 1 1"/>
</geometry> </geometry>
</visual> </visual>
<collision> <collision>
<origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/> <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
<geometry> <geometry>
<cylinder length="${hip_length}" radius="${hip_radius}"/> <cylinder length="${hip_length}" radius="${hip_radius}"/>
</geometry> </geometry>
</collision> </collision>
<inertial> <inertial>
<origin rpy="0 0 0" xyz="${hip_com_x*front_hind} ${hip_com_y*mirror} ${hip_com_z}"/> <origin rpy="0 0 0" xyz="${hip_com_x*front_hind} ${hip_com_y*mirror} ${hip_com_z}"/>
<mass value="${hip_mass}"/> <mass value="${hip_mass}"/>
<inertia <inertia
ixx="${hip_ixx}" ixy="${hip_ixy*mirror*front_hind}" ixz="${hip_ixz*front_hind}" ixx="${hip_ixx}" ixy="${hip_ixy*mirror*front_hind}" ixz="${hip_ixz*front_hind}"
iyy="${hip_iyy}" iyz="${hip_iyz*mirror}" iyy="${hip_iyy}" iyz="${hip_iyz*mirror}"
izz="${hip_izz}"/> izz="${hip_izz}"/>
</inertial> </inertial>
</link> </link>
<joint name="${name}_hip_fixed" type="fixed"> <joint name="${name}_hip_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 ${(thigh_shoulder_length/2.0+hip_offset)*mirror} 0"/> <origin rpy="0 0 0" xyz="0 ${(thigh_shoulder_length/2.0+hip_offset)*mirror} 0"/>
<parent link="${name}_hip"/> <parent link="${name}_hip"/>
<child link="${name}_thigh_shoulder"/> <child link="${name}_thigh_shoulder"/>
</joint> </joint>
<!-- this link is only for collision --> <!-- this link is only for collision -->
<link name="${name}_thigh_shoulder"> <link name="${name}_thigh_shoulder">
<collision> <collision>
<origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/> <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
<geometry> <geometry>
<cylinder length="${thigh_shoulder_length}" radius="${thigh_shoulder_radius}"/> <cylinder length="${thigh_shoulder_length}" radius="${thigh_shoulder_radius}"/>
</geometry> </geometry>
</collision> </collision>
</link> </link>
<joint name="${name}_thigh_joint" type="revolute"> <joint name="${name}_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 ${thigh_offset*mirror} 0"/> <origin rpy="0 0 0" xyz="0 ${thigh_offset*mirror} 0"/>
<parent link="${name}_hip"/> <parent link="${name}_hip"/>
<child link="${name}_thigh"/> <child link="${name}_thigh"/>
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
<dynamics damping="${damping}" friction="${friction}"/> <dynamics damping="${damping}" friction="${friction}"/>
<limit effort="${thigh_torque_max}" velocity="${thigh_velocity_max}" lower="${thigh_min*PI/180.0}" upper="${thigh_max*PI/180.0}"/> <limit effort="${thigh_torque_max}" velocity="${thigh_velocity_max}" lower="${thigh_min*PI/180.0}"
</joint> upper="${thigh_max*PI/180.0}"/>
</joint>
<link name="${name}_thigh"> <link name="${name}_thigh">
<visual> <visual>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>
<geometry> <geometry>
<xacro:if value="${mirror_dae == True}"> <xacro:if value="${mirror_dae == True}">
<mesh filename="file://$(find a1_description)/meshes/thigh.dae" scale="1 1 1"/> <mesh filename="file://$(find a1_description)/meshes/thigh.dae" scale="1 1 1"/>
</xacro:if> </xacro:if>
<xacro:if value="${mirror_dae == False}"> <xacro:if value="${mirror_dae == False}">
<mesh filename="file://$(find a1_description)/meshes/thigh_mirror.dae" scale="1 1 1"/> <mesh filename="file://$(find a1_description)/meshes/thigh_mirror.dae" scale="1 1 1"/>
</xacro:if> </xacro:if>
</geometry> </geometry>
</visual> </visual>
<collision> <collision>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-thigh_length/2.0}"/> <origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-thigh_length/2.0}"/>
<geometry> <geometry>
<box size="${thigh_length} ${thigh_width} ${thigh_height}"/> <box size="${thigh_length} ${thigh_width} ${thigh_height}"/>
</geometry> </geometry>
</collision> </collision>
<inertial> <inertial>
<origin rpy="0 0 0" xyz="${thigh_com_x} ${thigh_com_y*mirror} ${thigh_com_z}"/> <origin rpy="0 0 0" xyz="${thigh_com_x} ${thigh_com_y*mirror} ${thigh_com_z}"/>
<mass value="${thigh_mass}"/> <mass value="${thigh_mass}"/>
<inertia <inertia
ixx="${thigh_ixx}" ixy="${thigh_ixy*mirror}" ixz="${thigh_ixz}" ixx="${thigh_ixx}" ixy="${thigh_ixy*mirror}" ixz="${thigh_ixz}"
iyy="${thigh_iyy}" iyz="${thigh_iyz*mirror}" iyy="${thigh_iyy}" iyz="${thigh_iyz*mirror}"
izz="${thigh_izz}"/> izz="${thigh_izz}"/>
</inertial> </inertial>
</link> </link>
<joint name="${name}_calf_joint" type="revolute"> <joint name="${name}_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 ${-thigh_length}"/> <origin rpy="0 0 0" xyz="0 0 ${-thigh_length}"/>
<parent link="${name}_thigh"/> <parent link="${name}_thigh"/>
<child link="${name}_calf"/> <child link="${name}_calf"/>
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
<dynamics damping="${damping}" friction="${friction}"/> <dynamics damping="${damping}" friction="${friction}"/>
<limit effort="${calf_torque_max}" velocity="${calf_velocity_max}" lower="${calf_min*PI/180.0}" upper="${calf_max*PI/180.0}"/> <limit effort="${calf_torque_max}" velocity="${calf_velocity_max}" lower="${calf_min*PI/180.0}"
</joint> upper="${calf_max*PI/180.0}"/>
</joint>
<link name="${name}_calf"> <link name="${name}_calf">
<visual> <visual>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>
<geometry> <geometry>
<mesh filename="file://$(find a1_description)/meshes/calf.dae" scale="1 1 1"/> <mesh filename="file://$(find a1_description)/meshes/calf.dae" scale="1 1 1"/>
</geometry> </geometry>
</visual> </visual>
<collision> <collision>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-calf_length/2.0}"/> <origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-calf_length/2.0}"/>
<geometry> <geometry>
<box size="${calf_length} ${calf_width} ${calf_height}"/> <box size="${calf_length} ${calf_width} ${calf_height}"/>
</geometry> </geometry>
</collision> </collision>
<inertial> <inertial>
<origin rpy="0 0 0" xyz="${calf_com_x} ${calf_com_y} ${calf_com_z}"/> <origin rpy="0 0 0" xyz="${calf_com_x} ${calf_com_y} ${calf_com_z}"/>
<mass value="${calf_mass}"/> <mass value="${calf_mass}"/>
<inertia <inertia
ixx="${calf_ixx}" ixy="${calf_ixy}" ixz="${calf_ixz}" ixx="${calf_ixx}" ixy="${calf_ixy}" ixz="${calf_ixz}"
iyy="${calf_iyy}" iyz="${calf_iyz}" iyy="${calf_iyy}" iyz="${calf_iyz}"
izz="${calf_izz}"/> izz="${calf_izz}"/>
</inertial> </inertial>
</link> </link>
<joint name="${name}_foot_fixed" type="fixed"> <joint name="${name}_foot_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 ${-(calf_length)}"/> <origin rpy="0 0 0" xyz="0 0 ${-(calf_length)}"/>
<parent link="${name}_calf"/> <parent link="${name}_calf"/>
<child link="${name}_foot"/> <child link="${name}_foot"/>
</joint> </joint>
<link name="${name}_foot"> <link name="${name}_foot">
<visual> <visual>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>
<geometry> <geometry>
<sphere radius="${foot_radius-0.01}"/> <sphere radius="${foot_radius-0.01}"/>
</geometry> </geometry>
</visual> </visual>
<collision> <collision>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>
<geometry> <geometry>
<sphere radius="${foot_radius}"/> <sphere radius="${foot_radius}"/>
</geometry> </geometry>
</collision> </collision>
<inertial> <inertial>
<mass value="${foot_mass}"/> <mass value="${foot_mass}"/>
<inertia <inertia
ixx="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" ixy="0.0" ixz="0.0" ixx="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" ixy="0.0" ixz="0.0"
iyy="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" iyz="0.0" iyy="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" iyz="0.0"
izz="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}"/> izz="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}"/>
</inertial> </inertial>
</link> </link>
<gazebo reference="${name}_hip"> <gazebo reference="${name}_hip">
<mu1>0.2</mu1> <mu1>0.2</mu1>
<mu2>0.2</mu2> <mu2>0.2</mu2>
<visual> <visual>
<material> <material>
<ambient>.5 .5 .5 1.0</ambient> <ambient>.5 .5 .5 1.0</ambient>
<diffuse>.5 .5 .5 1.0</diffuse> <diffuse>.5 .5 .5 1.0</diffuse>
<specular>.5 .5 .5 1.0</specular> <specular>.5 .5 .5 1.0</specular>
</material> </material>
</visual> </visual>
</gazebo> </gazebo>
<gazebo reference="${name}_thigh"> <gazebo reference="${name}_thigh">
<mu1>0.2</mu1> <mu1>0.2</mu1>
<mu2>0.2</mu2> <mu2>0.2</mu2>
<self_collide>0</self_collide> <self_collide>0</self_collide>
<visual> <visual>
<material> <material>
<ambient>.175 .175 .175 1.0</ambient> <ambient>.175 .175 .175 1.0</ambient>
<diffuse>.175 .175 .175 1.0</diffuse> <diffuse>.175 .175 .175 1.0</diffuse>
<specular>.175 .175 .175 1.0</specular> <specular>.175 .175 .175 1.0</specular>
</material> </material>
</visual> </visual>
<kp value="1000000.0"/> <kp value="1000000.0"/>
<kd value="100.0"/> <kd value="100.0"/>
</gazebo> </gazebo>
<gazebo reference="${name}_calf"> <gazebo reference="${name}_calf">
<mu1>0.6</mu1> <mu1>0.6</mu1>
<mu2>0.6</mu2> <mu2>0.6</mu2>
<self_collide>1</self_collide> <self_collide>1</self_collide>
<visual> <visual>
<material> <material>
<ambient>.5 .5 .5 1.0</ambient> <ambient>.5 .5 .5 1.0</ambient>
<diffuse>.5 .5 .5 1.0</diffuse> <diffuse>.5 .5 .5 1.0</diffuse>
<specular>.5 .5 .5 1.0</specular> <specular>.5 .5 .5 1.0</specular>
</material> </material>
</visual> </visual>
</gazebo> </gazebo>
<gazebo reference="${name}_foot"> <gazebo reference="${name}_foot">
<mu1>0.6</mu1> <mu1>0.6</mu1>
<mu2>0.6</mu2> <mu2>0.6</mu2>
<self_collide>1</self_collide> <self_collide>1</self_collide>
<visual> <visual>
<material> <material>
<ambient>.175 .175 .175 1.0</ambient> <ambient>.175 .175 .175 1.0</ambient>
<diffuse>.175 .175 .175 1.0</diffuse> <diffuse>.175 .175 .175 1.0</diffuse>
<specular>.175 .175 .175 1.0</specular> <specular>.175 .175 .175 1.0</specular>
</material> </material>
</visual> </visual>
<kp value="1000000.0"/> <kp value="1000000.0"/>
<kd value="100.0"/> <kd value="100.0"/>
</gazebo> </gazebo>
<xacro:leg_transmission name="${name}"/> <xacro:leg_transmission name="${name}"/>
</xacro:macro> </xacro:macro>
</robot> </robot>

10
descriptions/unitree/go2_description/README.md
View File

@ -12,7 +12,7 @@ Tested environment:
## Build ## Build
```bash ```bash
cd ~/ros2_ws cd ~/ros2_ws
colcon build --packages-up-to go2_description colcon build --packages-up-to go2_description --symlink-install
``` ```
## Visualize the robot ## Visualize the robot
@ -23,6 +23,7 @@ ros2 launch go2_description visualize.launch.py
``` ```
## Launch ROS2 Control ## Launch ROS2 Control
### Mujoco Simulator
* Unitree Guide Controller * Unitree Guide Controller
```bash ```bash
source ~/ros2_ws/install/setup.bash source ~/ros2_ws/install/setup.bash
@ -34,6 +35,13 @@ ros2 launch go2_description visualize.launch.py
ros2 launch go2_description ocs2_control.launch.py ros2 launch go2_description ocs2_control.launch.py
``` ```
### Gazebo Simulator
* Unitree Guide Controller
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch go2_description gazebo_unitree_guide.launch.py
```
## When used for isaac gym or other similiar engine ## When used for isaac gym or other similiar engine
Collision parameters in urdf can be amended to better train the robot: Collision parameters in urdf can be amended to better train the robot:

142
descriptions/unitree/go2_description/config/gazebo.yaml Normal file
View File

@ -0,0 +1,142 @@
# Controller Manager configuration
controller_manager:
ros__parameters:
update_rate: 2000 # Hz
# Define the available controllers
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
imu_sensor_broadcaster:
type: imu_sensor_broadcaster/IMUSensorBroadcaster
leg_pd_controller:
type: leg_pd_controller/LegPdController
unitree_guide_controller:
type: unitree_guide_controller/UnitreeGuideController
legged_gym_controller:
type: legged_gym_controller/LeggedGymController
imu_sensor_broadcaster:
ros__parameters:
sensor_name: "imu_sensor"
frame_id: "imu_link"
leg_pd_controller:
ros__parameters:
joints:
- FR_hip_joint
- FR_thigh_joint
- FR_calf_joint
- FL_hip_joint
- FL_thigh_joint
- FL_calf_joint
- RR_hip_joint
- RR_thigh_joint
- RR_calf_joint
- RL_hip_joint
- RL_thigh_joint
- RL_calf_joint
command_interfaces:
- effort
state_interfaces:
- position
- velocity
unitree_guide_controller:
ros__parameters:
command_prefix: "leg_pd_controller"
joints:
- FR_hip_joint
- FR_thigh_joint
- FR_calf_joint
- FL_hip_joint
- FL_thigh_joint
- FL_calf_joint
- RR_hip_joint
- RR_thigh_joint
- RR_calf_joint
- RL_hip_joint
- RL_thigh_joint
- RL_calf_joint
command_interfaces:
- effort
- position
- velocity
- kp
- kd
state_interfaces:
- effort
- position
- velocity
feet_names:
- FR_foot
- FL_foot
- RR_foot
- RL_foot
imu_name: "imu_sensor"
base_name: "base"
imu_interfaces:
- orientation.w
- orientation.x
- orientation.y
- orientation.z
- angular_velocity.x
- angular_velocity.y
- angular_velocity.z
- linear_acceleration.x
- linear_acceleration.y
- linear_acceleration.z
legged_gym_controller:
ros__parameters:
update_rate: 200 # Hz
config_folder: "/home/tlab-uav/ros2_ws/install/a1_description/share/a1_description/config/issacgym"
command_prefix: "leg_pd_controller"
joints:
- FL_hip_joint
- FL_thigh_joint
- FL_calf_joint
- FR_hip_joint
- FR_thigh_joint
- FR_calf_joint
- RL_hip_joint
- RL_thigh_joint
- RL_calf_joint
- RR_hip_joint
- RR_thigh_joint
- RR_calf_joint
command_interfaces:
- effort
- position
- velocity
- kp
- kd
state_interfaces:
- effort
- position
- velocity
imu_name: "imu_sensor"
imu_interfaces:
- orientation.w
- orientation.x
- orientation.y
- orientation.z
- angular_velocity.x
- angular_velocity.y
- angular_velocity.z
- linear_acceleration.x
- linear_acceleration.y
- linear_acceleration.z

106
descriptions/unitree/go2_description/launch/gazebo_unitree_guide.launch.py Normal file
View File

@ -0,0 +1,106 @@
import os
import xacro
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.event_handlers import OnProcessExit
from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
package_description = "go2_description"
def process_xacro():
pkg_path = os.path.join(get_package_share_directory(package_description))
xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
robot_description_config = xacro.process_file(xacro_file, mappings={'GAZEBO': 'true'})
return robot_description_config.toxml()
def generate_launch_description():
rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
robot_description = process_xacro()
gz_spawn_entity = Node(
package='ros_gz_sim',
executable='create',
output='screen',
arguments=['-topic', 'robot_description', '-name',
'go2', '-allow_renaming', 'true', '-z', '0.5'],
)
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
parameters=[
{
'publish_frequency': 20.0,
'use_tf_static': True,
'robot_description': robot_description,
'ignore_timestamp': True
}
],
)
joint_state_publisher = Node(
package="controller_manager",
executable="spawner",
arguments=["joint_state_broadcaster",
"--controller-manager", "/controller_manager"],
)
imu_sensor_broadcaster = Node(
package="controller_manager",
executable="spawner",
arguments=["imu_sensor_broadcaster",
"--controller-manager", "/controller_manager"],
)
leg_pd_controller = Node(
package="controller_manager",
executable="spawner",
arguments=["leg_pd_controller",
"--controller-manager", "/controller_manager"],
)
unitree_guide_controller = Node(
package="controller_manager",
executable="spawner",
arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
)
return LaunchDescription([
Node(
package='rviz2',
executable='rviz2',
name='rviz_ocs2',
output='screen',
arguments=["-d", rviz_config_file]
),
Node(
package='ros_gz_bridge',
executable='parameter_bridge',
arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
output='screen'
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[PathJoinSubstitution([FindPackageShare('ros_gz_sim'),
'launch',
'gz_sim.launch.py'])]),
launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]),
robot_state_publisher,
gz_spawn_entity,
leg_pd_controller,
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=leg_pd_controller,
on_exit=[imu_sensor_broadcaster, joint_state_publisher, unitree_guide_controller],
)
),
])

405
descriptions/unitree/go2_description/xacro/gazebo.xacro
View File

@ -1,257 +1,188 @@
<?xml version="1.0"?> <?xml version="1.0"?>
<robot> <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- ros_control plugin -->
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/go2_gazebo</robotNamespace>
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
</plugin>
</gazebo>
<!-- Show the trajectory of trunk center. --> <ros2_control name="GazeboSystem" type="system">
<!--gazebo> <hardware>
<plugin name="3dplot" filename="libLinkPlot3DPlugin.so"> <plugin>gz_ros2_control/GazeboSimSystem</plugin>
<frequency>10</frequency> </hardware>
<plot>
<link>base</link>
<pose>0 0 0 0 0 0</pose>
<material>Gazebo/Yellow</material>
</plot>
</plugin>
</gazebo-->
<!-- Show the trajectory of foot. You can add another trajectory about another foot. --> <joint name="FR_hip_joint">
<gazebo> <command_interface name="effort" initial_value="0.0"/>
<plugin name="3dplot" filename="libLinkPlot3DPlugin.so"> <state_interface name="position"/>
<frequency>100</frequency> <state_interface name="velocity"/>
<plot> <state_interface name="effort"/>
<link>FL_foot</link> </joint>
<pose>0 0 0 0 0 0</pose>
</plot>
</plugin>
</gazebo>
<gazebo> <joint name="FR_thigh_joint">
<plugin filename="libgazebo_ros_force.so" name="gazebo_ros_force"> <command_interface name="effort" initial_value="0.0"/>
<bodyName>trunk</bodyName> <state_interface name="position"/>
<topicName>/apply_force/trunk</topicName> <state_interface name="velocity"/>
</plugin> <state_interface name="effort"/>
</gazebo> </joint>
<gazebo reference="imu_link"> <joint name="FR_calf_joint">
<gravity>true</gravity> <command_interface name="effort" initial_value="0.0"/>
<sensor name="imu_sensor" type="imu"> <state_interface name="position"/>
<always_on>true</always_on> <state_interface name="velocity"/>
<update_rate>1000</update_rate> <state_interface name="effort"/>
<visualize>true</visualize> </joint>
<topic>__default_topic__</topic>
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
<topicName>trunk_imu</topicName>
<bodyName>imu_link</bodyName>
<updateRateHZ>1000.0</updateRateHZ>
<gaussianNoise>0.0</gaussianNoise>
<xyzOffset>0 0 0</xyzOffset>
<rpyOffset>0 0 0</rpyOffset>
<frameName>imu_link</frameName>
</plugin>
<pose>0 0 0 0 0 0</pose>
</sensor>
</gazebo>
<joint name="FL_hip_joint">
<command_interface name="effort" initial_value="0.0"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FL_thigh_joint">
<command_interface name="effort" initial_value="0.0"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FL_calf_joint">
<command_interface name="effort" initial_value="0.0"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RR_hip_joint">
<command_interface name="effort" initial_value="0.0"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RR_thigh_joint">
<command_interface name="effort" initial_value="0.0"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RR_calf_joint">
<command_interface name="effort" initial_value="0.0"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<!-- Foot contacts. --> <joint name="RL_hip_joint">
<gazebo reference="FR_calf"> <command_interface name="effort" initial_value="0.0"/>
<sensor name="FR_foot_contact" type="contact"> <state_interface name="position"/>
<update_rate>100</update_rate> <state_interface name="velocity"/>
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/> <state_interface name="effort"/>
<contact> </joint>
<collision>FR_calf_fixed_joint_lump__FR_foot_collision_1</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="FL_calf">
<sensor name="FL_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
<contact>
<collision>FL_calf_fixed_joint_lump__FL_foot_collision_1</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="RR_calf">
<sensor name="RR_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
<contact>
<collision>RR_calf_fixed_joint_lump__RR_foot_collision_1</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="RL_calf">
<sensor name="RL_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
<contact>
<collision>RL_calf_fixed_joint_lump__RL_foot_collision_1</collision>
</contact>
</sensor>
</gazebo>
<!-- Visualization of Foot contacts. --> <joint name="RL_thigh_joint">
<gazebo reference="FR_foot"> <command_interface name="effort" initial_value="0.0"/>
<visual> <state_interface name="position"/>
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so"> <state_interface name="velocity"/>
<topicName>FR_foot_contact</topicName> <state_interface name="effort"/>
</plugin> </joint>
</visual>
</gazebo>
<gazebo reference="FL_foot">
<visual>
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
<topicName>FL_foot_contact</topicName>
</plugin>
</visual>
</gazebo>
<gazebo reference="RR_foot">
<visual>
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
<topicName>RR_foot_contact</topicName>
</plugin>
</visual>
</gazebo>
<gazebo reference="RL_foot">
<visual>
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
<topicName>RL_foot_contact</topicName>
</plugin>
</visual>
</gazebo>
<gazebo reference="base"> <joint name="RL_calf_joint">
<turnGravityOff>false</turnGravityOff> <command_interface name="effort" initial_value="0.0"/>
</gazebo> <state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<gazebo reference="trunk"> <sensor name="imu_sensor">
<mu1>0.2</mu1> <state_interface name="orientation.x"/>
<mu2>0.2</mu2> <state_interface name="orientation.y"/>
<kp value="1000000.0"/> <state_interface name="orientation.z"/>
<kd value="1.0"/> <state_interface name="orientation.w"/>
</gazebo> <state_interface name="angular_velocity.x"/>
<state_interface name="angular_velocity.y"/>
<state_interface name="angular_velocity.z"/>
<state_interface name="linear_acceleration.x"/>
<state_interface name="linear_acceleration.y"/>
<state_interface name="linear_acceleration.z"/>
</sensor>
</ros2_control>
<gazebo reference="stick_link"> <gazebo>
<mu1>0.2</mu1> <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
<mu2>0.2</mu2> <parameters>$(find a1_description)/config/gazebo.yaml</parameters>
</gazebo> </plugin>
<plugin filename="gz-sim-imu-system"
name="gz::sim::systems::Imu">
</plugin>
</gazebo>
<gazebo reference="imu_link"> <gazebo reference="trunk">
<mu1>0.2</mu1> <mu1>0.6</mu1>
<mu2>0.2</mu2> <mu2>0.6</mu2>
</gazebo> <self_collide>1</self_collide>
</gazebo>
<!-- FL leg --> <gazebo reference="imu_link">
<gazebo reference="FL_hip"> <sensor name="imu_sensor" type="imu">
<mu1>0.2</mu1> <always_on>1</always_on>
<mu2>0.2</mu2> <update_rate>500</update_rate>
<visualize>true</visualize>
</gazebo> <topic>imu</topic>
<gazebo reference="FL_thigh"> <imu>
<mu1>0.2</mu1> <angular_velocity>
<mu2>0.2</mu2> <x>
<self_collide>1</self_collide> <!-- <noise type="gaussian">-->
<kp value="1000000.0"/> <!-- <mean>0.0</mean>-->
<kd value="1.0"/> <!-- <stddev>2e-4</stddev>-->
</gazebo> <!-- <bias_mean>0.0000075</bias_mean>-->
<gazebo reference="FL_calf"> <!-- <bias_stddev>0.0000008</bias_stddev>-->
<mu1>0.2</mu1> <!-- </noise>-->
<mu2>0.2</mu2> </x>
<self_collide>1</self_collide> <y>
</gazebo> <!-- <noise type="gaussian">-->
<gazebo reference="FL_foot"> <!-- <mean>0.0</mean>-->
<mu1>0.6</mu1> <!-- <stddev>2e-4</stddev>-->
<mu2>0.6</mu2> <!-- <bias_mean>0.0000075</bias_mean>-->
<self_collide>1</self_collide> <!-- <bias_stddev>0.0000008</bias_stddev>-->
<kp value="1000000.0"/> <!-- </noise>-->
<kd value="1.0"/> </y>
</gazebo> <z>
<!-- <noise type="gaussian">-->
<!-- FR leg --> <!-- <mean>0.0</mean>-->
<gazebo reference="FR_hip"> <!-- <stddev>2e-4</stddev>-->
<mu1>0.2</mu1> <!-- <bias_mean>0.0000075</bias_mean>-->
<mu2>0.2</mu2> <!-- <bias_stddev>0.0000008</bias_stddev>-->
</gazebo> <!-- </noise>-->
<gazebo reference="FR_thigh"> </z>
<mu1>0.2</mu1> </angular_velocity>
<mu2>0.2</mu2> <linear_acceleration>
<self_collide>1</self_collide> <x>
<kp value="1000000.0"/> <!-- <noise type="gaussian">-->
<kd value="1.0"/> <!-- <mean>0.0</mean>-->
</gazebo> <!-- <stddev>1.7e-2</stddev>-->
<gazebo reference="FR_calf"> <!-- <bias_mean>0.1</bias_mean>-->
<mu1>0.2</mu1> <!-- <bias_stddev>0.001</bias_stddev>-->
<mu2>0.2</mu2> <!-- </noise>-->
<self_collide>1</self_collide> </x>
</gazebo> <y>
<gazebo reference="FR_foot"> <!-- <noise type="gaussian">-->
<mu1>0.6</mu1> <!-- <mean>0.0</mean>-->
<mu2>0.6</mu2> <!-- <stddev>1.7e-2</stddev>-->
<self_collide>1</self_collide> <!-- <bias_mean>0.1</bias_mean>-->
<kp value="1000000.0"/> <!-- <bias_stddev>0.001</bias_stddev>-->
<kd value="1.0"/> <!-- </noise>-->
</gazebo> </y>
<z>
<!-- RL leg --> <!-- <noise type="gaussian">-->
<gazebo reference="RL_hip"> <!-- <mean>0.0</mean>-->
<mu1>0.2</mu1> <!-- <stddev>1.7e-2</stddev>-->
<mu2>0.2</mu2> <!-- <bias_mean>0.1</bias_mean>-->
</gazebo> <!-- <bias_stddev>0.001</bias_stddev>-->
<gazebo reference="RL_thigh"> <!-- </noise>-->
<mu1>0.2</mu1> </z>
<mu2>0.2</mu2> </linear_acceleration>
<self_collide>1</self_collide> </imu>
<kp value="1000000.0"/> </sensor>
<kd value="1.0"/> </gazebo>
</gazebo> <gazebo reference="imu_joint">
<gazebo reference="RL_calf"> <disableFixedJointLumping>true</disableFixedJointLumping>
<mu1>0.2</mu1> </gazebo>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="RL_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<!-- RR leg -->
<gazebo reference="RR_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
</gazebo>
<gazebo reference="RR_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="RR_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="RR_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
</robot> </robot>

339
descriptions/unitree/go2_description/xacro/leg.xacro
View File

@ -2,175 +2,202 @@
<robot xmlns:xacro="http://www.ros.org/wiki/xacro"> <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find go2_description)/xacro/transmission.xacro"/> <xacro:include filename="$(find go2_description)/xacro/transmission.xacro"/>
<xacro:macro name="leg" params="name mirror mirror_dae front_hind front_hind_dae"> <xacro:macro name="leg" params="name mirror mirror_dae front_hind front_hind_dae">
<joint name="${name}_hip_joint" type="revolute"> <joint name="${name}_hip_joint" type="revolute">
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}"> <xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="${leg_offset_x} ${-leg_offset_y} 0"/> <origin rpy="0 0 0" xyz="${leg_offset_x} ${-leg_offset_y} 0"/>
</xacro:if> </xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}"> <xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="${leg_offset_x} ${leg_offset_y} 0"/> <origin rpy="0 0 0" xyz="${leg_offset_x} ${leg_offset_y} 0"/>
</xacro:if> </xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}"> <xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
<origin rpy="0 0 0" xyz="${-leg_offset_x} ${-leg_offset_y} 0"/> <origin rpy="0 0 0" xyz="${-leg_offset_x} ${-leg_offset_y} 0"/>
</xacro:if> </xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}"> <xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
<origin rpy="0 0 0" xyz="${-leg_offset_x} ${leg_offset_y} 0"/> <origin rpy="0 0 0" xyz="${-leg_offset_x} ${leg_offset_y} 0"/>
</xacro:if> </xacro:if>
<parent link="trunk"/> <parent link="trunk"/>
<child link="${name}_hip"/> <child link="${name}_hip"/>
<axis xyz="1 0 0"/> <axis xyz="1 0 0"/>
<dynamics damping="${damping}" friction="${friction}"/> <dynamics damping="${damping}" friction="${friction}"/>
<xacro:if value="${(mirror_dae == True)}"> <xacro:if value="${(mirror_dae == True)}">
<limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${hip_position_min}" <limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${hip_position_min}"
upper="${hip_position_max}"/> upper="${hip_position_max}"/>
</xacro:if> </xacro:if>
<xacro:if value="${(mirror_dae == False)}"> <xacro:if value="${(mirror_dae == False)}">
<limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${-hip_position_max}" <limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${-hip_position_max}"
upper="${-hip_position_min}"/> upper="${-hip_position_min}"/>
</xacro:if> </xacro:if>
</joint> </joint>
<link name="${name}_hip"> <link name="${name}_hip">
<visual> <visual>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}"> <xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>
</xacro:if> </xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}"> <xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
<origin rpy="${PI} 0 0" xyz="0 0 0"/> <origin rpy="${PI} 0 0" xyz="0 0 0"/>
</xacro:if> </xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}"> <xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
<origin rpy="0 ${PI} 0" xyz="0 0 0"/> <origin rpy="0 ${PI} 0" xyz="0 0 0"/>
</xacro:if> </xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}"> <xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
<origin rpy="${PI} ${PI} 0" xyz="0 0 0"/> <origin rpy="${PI} ${PI} 0" xyz="0 0 0"/>
</xacro:if> </xacro:if>
<geometry> <geometry>
<mesh filename="file://$(find go2_description)/meshes/hip.dae" scale="1 1 1"/> <mesh filename="file://$(find go2_description)/meshes/hip.dae" scale="1 1 1"/>
</geometry> </geometry>
</visual> </visual>
<collision> <collision>
<origin rpy="${PI/2.0} 0 0" xyz="0 ${hip_offset*mirror} 0"/> <origin rpy="${PI/2.0} 0 0" xyz="0 ${hip_offset*mirror} 0"/>
<geometry> <geometry>
<cylinder length="${hip_length}" radius="${hip_radius}"/> <cylinder length="${hip_length}" radius="${hip_radius}"/>
</geometry> </geometry>
</collision> </collision>
<inertial> <inertial>
<origin rpy="0 0 0" xyz="${hip_com_x*front_hind} ${hip_com_y*mirror} ${hip_com_z}"/> <origin rpy="0 0 0" xyz="${hip_com_x*front_hind} ${hip_com_y*mirror} ${hip_com_z}"/>
<mass value="${hip_mass}"/> <mass value="${hip_mass}"/>
<inertia <inertia
ixx="${hip_ixx}" ixy="${hip_ixy*mirror*front_hind}" ixz="${hip_ixz*front_hind}" ixx="${hip_ixx}" ixy="${hip_ixy*mirror*front_hind}" ixz="${hip_ixz*front_hind}"
iyy="${hip_iyy}" iyz="${hip_iyz*mirror}" iyy="${hip_iyy}" iyz="${hip_iyz*mirror}"
izz="${hip_izz}"/> izz="${hip_izz}"/>
</inertial> </inertial>
</link> </link>
<joint name="${name}_thigh_joint" type="revolute"> <joint name="${name}_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 ${thigh_offset*mirror} 0"/> <origin rpy="0 0 0" xyz="0 ${thigh_offset*mirror} 0"/>
<parent link="${name}_hip"/> <parent link="${name}_hip"/>
<child link="${name}_thigh"/> <child link="${name}_thigh"/>
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
<dynamics damping="${damping}" friction="${friction}"/> <dynamics damping="${damping}" friction="${friction}"/>
<limit effort="${thigh_torque_max}" velocity="${thigh_velocity_max}" lower="${thigh_position_min}" <limit effort="${thigh_torque_max}" velocity="${thigh_velocity_max}" lower="${thigh_position_min}"
upper="${thigh_position_max}"/> upper="${thigh_position_max}"/>
</joint> </joint>
<link name="${name}_thigh"> <link name="${name}_thigh">
<visual> <visual>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>
<geometry> <geometry>
<xacro:if value="${mirror_dae == True}"> <xacro:if value="${mirror_dae == True}">
<mesh filename="file://$(find go2_description)/meshes/thigh.dae" scale="1 1 1"/> <mesh filename="file://$(find go2_description)/meshes/thigh.dae" scale="1 1 1"/>
</xacro:if> </xacro:if>
<xacro:if value="${mirror_dae == False}"> <xacro:if value="${mirror_dae == False}">
<mesh filename="file://$(find go2_description)/meshes/thigh_mirror.dae" scale="1 1 1"/> <mesh filename="file://$(find go2_description)/meshes/thigh_mirror.dae" scale="1 1 1"/>
</xacro:if> </xacro:if>
</geometry> </geometry>
</visual> </visual>
<collision> <collision>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-thigh_length/2.0}"/> <origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-thigh_length/2.0}"/>
<geometry> <geometry>
<box size="${thigh_length} ${thigh_width} ${thigh_height}"/> <box size="${thigh_length} ${thigh_width} ${thigh_height}"/>
</geometry> </geometry>
</collision> </collision>
<inertial> <inertial>
<origin rpy="0 0 0" xyz="${thigh_com_x} ${thigh_com_y*mirror} ${thigh_com_z}"/> <origin rpy="0 0 0" xyz="${thigh_com_x} ${thigh_com_y*mirror} ${thigh_com_z}"/>
<mass value="${thigh_mass}"/> <mass value="${thigh_mass}"/>
<inertia <inertia
ixx="${thigh_ixx}" ixy="${thigh_ixy*mirror}" ixz="${thigh_ixz}" ixx="${thigh_ixx}" ixy="${thigh_ixy*mirror}" ixz="${thigh_ixz}"
iyy="${thigh_iyy}" iyz="${thigh_iyz*mirror}" iyy="${thigh_iyy}" iyz="${thigh_iyz*mirror}"
izz="${thigh_izz}"/> izz="${thigh_izz}"/>
</inertial> </inertial>
</link> </link>
<joint name="${name}_calf_joint" type="revolute"> <joint name="${name}_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 ${-thigh_length}"/> <origin rpy="0 0 0" xyz="0 0 ${-thigh_length}"/>
<parent link="${name}_thigh"/> <parent link="${name}_thigh"/>
<child link="${name}_calf"/> <child link="${name}_calf"/>
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
<dynamics damping="${damping}" friction="${friction}"/> <dynamics damping="${damping}" friction="${friction}"/>
<limit effort="${calf_torque_max}" velocity="${calf_velocity_max}" lower="${calf_position_min}" <limit effort="${calf_torque_max}" velocity="${calf_velocity_max}" lower="${calf_position_min}"
upper="${calf_position_max}"/> upper="${calf_position_max}"/>
</joint> </joint>
<link name="${name}_calf"> <link name="${name}_calf">
<visual> <visual>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>
<geometry> <geometry>
<mesh filename="file://$(find go2_description)/meshes/calf.dae" scale="1 1 1"/> <mesh filename="file://$(find go2_description)/meshes/calf.dae" scale="1 1 1"/>
</geometry> </geometry>
</visual> </visual>
<collision> <collision>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-calf_length/2.0}"/> <origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-calf_length/2.0}"/>
<geometry> <geometry>
<box size="${calf_length} ${calf_width} ${calf_height}"/> <box size="${calf_length} ${calf_width} ${calf_height}"/>
</geometry> </geometry>
</collision> </collision>
<inertial> <inertial>
<origin rpy="0 0 0" xyz="${calf_com_x} ${calf_com_y} ${calf_com_z}"/> <origin rpy="0 0 0" xyz="${calf_com_x} ${calf_com_y} ${calf_com_z}"/>
<mass value="${calf_mass}"/> <mass value="${calf_mass}"/>
<inertia <inertia
ixx="${calf_ixx}" ixy="${calf_ixy}" ixz="${calf_ixz}" ixx="${calf_ixx}" ixy="${calf_ixy}" ixz="${calf_ixz}"
iyy="${calf_iyy}" iyz="${calf_iyz}" iyy="${calf_iyy}" iyz="${calf_iyz}"
izz="${calf_izz}"/> izz="${calf_izz}"/>
</inertial> </inertial>
</link> </link>
<joint name="${name}_foot_fixed" type="fixed">0 <joint name="${name}_foot_fixed" type="fixed">0
<origin rpy="0 0 0" xyz="0 0 ${-(calf_length)}"/> <origin rpy="0 0 0" xyz="0 0 ${-(calf_length)}"/>
<parent link="${name}_calf"/> <parent link="${name}_calf"/>
<child link="${name}_foot"/> <child link="${name}_foot"/>
</joint> </joint>
<link name="${name}_foot"> <link name="${name}_foot">
<visual> <visual>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>
<geometry> <geometry>
<sphere radius="${foot_radius-0.01}"/> <sphere radius="${foot_radius-0.01}"/>
</geometry> </geometry>
</visual> </visual>
<collision> <collision>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>
<geometry> <geometry>
<sphere radius="${foot_radius}"/> <sphere radius="${foot_radius}"/>
</geometry> </geometry>
</collision> </collision>
<inertial> <inertial>
<mass value="${foot_mass}"/> <mass value="${foot_mass}"/>
<inertia <inertia
ixx="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" ixy="0.0" ixz="0.0" ixx="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" ixy="0.0" ixz="0.0"
iyy="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" iyz="0.0" iyy="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" iyz="0.0"
izz="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}"/> izz="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}"/>
</inertial> </inertial>
</link> </link>
<xacro:leg_transmission name="${name}"/> <gazebo reference="${name}_hip">
</xacro:macro> <mu1>0.2</mu1>
<mu2>0.2</mu2>
</gazebo>
<gazebo reference="${name}_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>0</self_collide>
<kp value="1000000.0"/>
<kd value="100.0"/>
</gazebo>
<gazebo reference="${name}_calf">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="${name}_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<kp value="1000000.0"/>
<kd value="100.0"/>
</gazebo>
<xacro:leg_transmission name="${name}"/>
</xacro:macro>
</robot> </robot>

10
descriptions/unitree/go2_description/xacro/robot.xacro
View File

@ -3,11 +3,19 @@
<robot name="go2" xmlns:xacro="http://www.ros.org/wiki/xacro"> <robot name="go2" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:arg name="DEBUG" default="false"/> <xacro:arg name="DEBUG" default="false"/>
<xacro:arg name="GAZEBO" default="false"/>
<xacro:include filename="$(find go2_description)/xacro/const.xacro"/> <xacro:include filename="$(find go2_description)/xacro/const.xacro"/>
<xacro:include filename="$(find go2_description)/xacro/materials.xacro"/> <xacro:include filename="$(find go2_description)/xacro/materials.xacro"/>
<xacro:include filename="$(find go2_description)/xacro/leg.xacro"/> <xacro:include filename="$(find go2_description)/xacro/leg.xacro"/>
<xacro:include filename="$(find go2_description)/xacro/ros2_control.xacro"/>
<xacro:if value="$(arg GAZEBO)">
<xacro:include filename="$(find go2_description)/xacro/gazebo.xacro"/>
</xacro:if>
<xacro:unless value="$(arg GAZEBO)">
<xacro:include filename="$(find go2_description)/xacro/ros2_control.xacro"/>
</xacro:unless>
<!-- <xacro:stairs stairs="15" xpos="0" ypos="0" zpos="0" /> --> <!-- <xacro:stairs stairs="15" xpos="0" ypos="0" zpos="0" /> -->