EmbodiedAI
/
rl_sar
mirror of https://github.com/fan-ziqi/rl_sar.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

add go2

This commit is contained in:
zma69650 2024-10-12 13:46:21 +08:00
parent 8b6a2d6af1
commit 9ee3d58953
36 changed files with 4399 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
src/rl_sar/launch/gazebo_go2_isaacgym.launch Normal file
View File

@ -0,0 +1,54 @@
<launch>
<arg name="wname" default="stairs"/>
<arg name="rname" default="go2"/>
<param name="robot_name" type="str" value="$(arg rname)_isaacgym"/>
<param name="ros_namespace" type="str" value="/$(arg rname)_gazebo/"/>
<param name="gazebo_model_name" type="str" value="$(arg rname)_gazebo"/>
<arg name="robot_path" value="(find $(arg rname)_description)"/>
<arg name="dollar" value="$"/>
<arg name="paused" default="true"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
<arg name="user_debug" default="false"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find rl_sar)/worlds/$(arg wname).world"/>
<arg name="debug" value="$(arg debug)"/>
<arg name="gui" value="$(arg gui)"/>
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="headless" value="$(arg headless)"/>
</include>
<!-- Load the URDF into the ROS Parameter Server -->
<param name="robot_description"
command="$(find xacro)/xacro --inorder '$(arg dollar)$(arg robot_path)/xacro/robot.xacro'
DEBUG:=$(arg user_debug)"/>
<!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
<!-- Set trunk and joint positions at startup -->
<node pkg="gazebo_ros" type="spawn_model" name="urdf_spawner" respawn="false" output="screen"
args="-urdf -z 0.6 -model $(arg rname)_gazebo -param robot_description"/>
<!-- Load joint controller configurations from YAML file to parameter server -->
<rosparam file="$(arg dollar)$(arg robot_path)/config/robot_control.yaml" command="load"/>
<!-- load the controllers -->
<node pkg="controller_manager" type="spawner" name="controller_spawner" respawn="false"
output="screen" ns="/$(arg rname)_gazebo" args="joint_state_controller
FL_hip_controller FL_thigh_controller FL_calf_controller
FR_hip_controller FR_thigh_controller FR_calf_controller
RL_hip_controller RL_thigh_controller RL_calf_controller
RR_hip_controller RR_thigh_controller RR_calf_controller "/>
<!-- convert joint states to TF transforms for rviz, etc -->
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"
respawn="false" output="screen">
<remap from="/joint_states" to="/$(arg rname)_gazebo/joint_states"/>
</node>
</launch>

56
src/rl_sar/models/go2_isaacgym/config.yaml Normal file
View File

@ -0,0 +1,56 @@
go2_isaacgym:
model_name: "himloco_3.pt"
framework: "isaacgym"
rows: 4
cols: 3
use_history: True
dt: 0.005
decimation: 4
num_observations: 45
observations: ["commands","ang_vel", "gravity_vec", "dof_pos", "dof_vel", "actions"]
clip_obs: 100.0
clip_actions_lower: [-100, -100, -100,
-100, -100, -100,
-100, -100, -100,
-100, -100, -100]
clip_actions_upper: [100, 100, 100,
100, 100, 100,
100, 100, 100,
100, 100, 100]
rl_kp: [30, 30, 30,
30, 30, 30,
30, 30, 30,
30, 30, 30]
rl_kd: [0.75, 0.75, 0.75,
0.75, 0.75, 0.75,
0.75, 0.75, 0.75,
0.75, 0.75, 0.75]
fixed_kp: [60, 60, 60,
60, 60, 60,
60, 60, 60,
60, 60, 60]
fixed_kd: [5, 5, 5,
5, 5, 5,
5, 5, 5,
5, 5, 5]
hip_scale_reduction: 1.0
hip_scale_reduction_indices: [0, 3, 6, 9]
num_of_dofs: 12
action_scale: 0.25
lin_vel_scale: 2.0
ang_vel_scale: 0.25
dof_pos_scale: 1.0
dof_vel_scale: 0.05
commands_scale: [2.0, 2.0, 0.25]
torque_limits: [33.5, 33.5, 33.5,
33.5, 33.5, 33.5,
33.5, 33.5, 33.5,
33.5, 33.5, 33.5]
default_dof_pos: [ 0.1000, 0.8000, -1.5000,
-0.1000, 0.8000, -1.5000,
0.1000, 1.0000, -1.5000,
-0.1000, 1.0000, -1.5000]
joint_controller_names: ["FL_hip_controller", "FL_thigh_controller", "FL_calf_controller",
"FR_hip_controller", "FR_thigh_controller", "FR_calf_controller",
"RL_hip_controller", "RL_thigh_controller", "RL_calf_controller",
"RR_hip_controller", "RR_thigh_controller", "RR_calf_controller"]

BIN
src/rl_sar/models/go2_isaacgym/himloco.pt Normal file
View File

Binary file not shown.

BIN
src/rl_sar/models/go2_isaacgym/himloco_2.pt Normal file
View File

Binary file not shown.

BIN
src/rl_sar/models/go2_isaacgym/himloco_3.pt Normal file
View File

Binary file not shown.

2
src/rl_sar/src/rl_sim.cpp
View File

@ -252,7 +252,7 @@ torch::Tensor RL_Sim::Forward()
if (this->params.use_history)
{
this->history_obs_buf.insert(clamped_obs);
this->history_obs = this->history_obs_buf.get_obs_vec({0, 1, 2, 3, 4, 5});
this->history_obs = this->history_obs_buf.get_obs_vec({5, 4, 3, 2, 1, 0});
actions = this->model.forward({this->history_obs}).toTensor();
}
else

14
src/robots/go2_description/CMakeLists.txt Normal file
View File

@ -0,0 +1,14 @@
cmake_minimum_required(VERSION 2.8.3)
project(go2_description)
find_package(catkin REQUIRED)
catkin_package()
find_package(roslaunch)
foreach(dir config launch meshes urdf)
install(DIRECTORY ${dir}/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
endforeach(dir)

1
src/robots/go2_description/config/joint_names_go2_description.yaml Normal file
View File

@ -0,0 +1 @@
controller_joint_names: ['', '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', ]

70
src/robots/go2_description/config/robot_control.yaml Executable file
View File

@ -0,0 +1,70 @@
go2_gazebo:
# Publish all joint states -----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 1000
# FL Controllers ---------------------------------------
FL_hip_controller:
type: robot_joint_controller/RobotJointController
joint: FL_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0}
FL_thigh_controller:
type: robot_joint_controller/RobotJointController
joint: FL_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
FL_calf_controller:
type: robot_joint_controller/RobotJointController
joint: FL_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
# FR Controllers ---------------------------------------
FR_hip_controller:
type: robot_joint_controller/RobotJointController
joint: FR_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0}
FR_thigh_controller:
type: robot_joint_controller/RobotJointController
joint: FR_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
FR_calf_controller:
type: robot_joint_controller/RobotJointController
joint: FR_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
# RL Controllers ---------------------------------------
RL_hip_controller:
type: robot_joint_controller/RobotJointController
joint: RL_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0}
RL_thigh_controller:
type: robot_joint_controller/RobotJointController
joint: RL_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
RL_calf_controller:
type: robot_joint_controller/RobotJointController
joint: RL_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
# RR Controllers ---------------------------------------
RR_hip_controller:
type: robot_joint_controller/RobotJointController
joint: RR_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0}
RR_thigh_controller:
type: robot_joint_controller/RobotJointController
joint: RR_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
RR_calf_controller:
type: robot_joint_controller/RobotJointController
joint: RR_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0}

217
src/robots/go2_description/dae/base.dae Normal file
View File

File diff suppressed because one or more lines are too long

130
src/robots/go2_description/dae/calf.dae Normal file
View File

File diff suppressed because one or more lines are too long

130
src/robots/go2_description/dae/calf_mirror.dae Normal file
View File

File diff suppressed because one or more lines are too long

89
src/robots/go2_description/dae/foot.dae Normal file
View File

File diff suppressed because one or more lines are too long

130
src/robots/go2_description/dae/hip.dae Normal file
View File

File diff suppressed because one or more lines are too long

130
src/robots/go2_description/dae/thigh.dae Normal file
View File

File diff suppressed because one or more lines are too long

130
src/robots/go2_description/dae/thigh_mirror.dae Normal file
View File

File diff suppressed because one or more lines are too long

250
src/robots/go2_description/launch/check_joint.rviz Normal file
View File

@ -0,0 +1,250 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /TF1/Frames1
Splitter Ratio: 0.5
Tree Height: 746
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Name: Time
SyncMode: 0
SyncSource: ""
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 85; 87; 83
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
FL_calf:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FL_foot:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FL_hip:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_calf:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_foot:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_hip:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_thigh:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Fl_thigh:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Link Tree Style: Links in Alphabetic Order
RL_calf:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_foot:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_hip:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_thigh:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_calf:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_foot:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_hip:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_thigh:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
base:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
imu:
Alpha: 1
Show Axes: false
Show Trail: false
radar:
Alpha: 1
Show Axes: false
Show Trail: false
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Class: rviz/TF
Enabled: false
Frame Timeout: 15
Frames:
All Enabled: true
Marker Alpha: 1
Marker Scale: 0.4000000059604645
Name: TF
Show Arrows: false
Show Axes: true
Show Names: true
Tree:
{}
Update Interval: 0
Value: false
- Class: rviz/MarkerArray
Enabled: true
Marker Topic: /link_inertias_viz
Name: MarkerArray
Namespaces:
{}
Queue Size: 100
Value: true
Enabled: true
Global Options:
Background Color: 238; 238; 236
Default Light: true
Fixed Frame: base
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: /initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 0.7113326191902161
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: 0.14559818804264069
Y: 0.02902654930949211
Z: -0.1431877315044403
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.5047972798347473
Target Frame: <Fixed Frame>
Yaw: 0.7604149580001831
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1043
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000015600000375fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000375000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000375fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000375000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000006240000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1920
X: 1920
Y: 0

20
src/robots/go2_description/launch/gazebo.launch Normal file
View File

@ -0,0 +1,20 @@
<launch>
<include
file="$(find gazebo_ros)/launch/empty_world.launch" />
<node
name="tf_footprint_base"
pkg="tf"
type="static_transform_publisher"
args="0 0 0 0 0 0 base_link base_footprint 40" />
<node
name="spawn_model"
pkg="gazebo_ros"
type="spawn_model"
args="-file $(find go2_description)/urdf/go2_description.urdf -urdf -model go2_description"
output="screen" />
<node
name="fake_joint_calibration"
pkg="rostopic"
type="rostopic"
args="pub /calibrated std_msgs/Bool true" />
</launch>

22
src/robots/go2_description/launch/go2_rviz.launch Normal file
View File

@ -0,0 +1,22 @@
<launch>
<arg name="user_debug" default="false"/>
<param name="robot_description" textfile="$(find go2_description)/urdf/go2_description.urdf" />
<!-- for higher robot_state_publisher average rate-->
<!-- <param name="rate" value="1000"/> -->
<!-- send fake joint values -->
<node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui">
<param name="use_gui" value="TRUE"/>
</node>
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
<param name="publish_frequency" type="double" value="1000.0"/>
</node>
<node pkg="rviz" type="rviz" name="rviz" respawn="false" output="screen"
args="-d $(find go2_description)/launch/check_joint.rviz"/>
</launch>

130
src/robots/go2_description/meshes/calf.dae Executable file
View File

File diff suppressed because one or more lines are too long

130
src/robots/go2_description/meshes/calf_mirror.dae Executable file
View File

File diff suppressed because one or more lines are too long

89
src/robots/go2_description/meshes/foot.dae Executable file
View File

File diff suppressed because one or more lines are too long

130
src/robots/go2_description/meshes/hip.dae Executable file
View File

File diff suppressed because one or more lines are too long

130
src/robots/go2_description/meshes/thigh.dae Executable file
View File

File diff suppressed because one or more lines are too long

130
src/robots/go2_description/meshes/thigh_mirror.dae Executable file
View File

File diff suppressed because one or more lines are too long

217
src/robots/go2_description/meshes/trunk.dae Executable file
View File

File diff suppressed because one or more lines are too long

21
src/robots/go2_description/package.xml Normal file
View File

@ -0,0 +1,21 @@
<package format="2">
<name>go2_description</name>
<version>1.0.0</version>
<description>
<p>URDF Description package for go2_description</p>
<p>This package contains configuration data, 3D models and launch files
for go2_description robot</p>
</description>
<author>TODO</author>
<maintainer email="TODO@email.com" />
<license>BSD</license>
<buildtool_depend>catkin</buildtool_depend>
<depend>roslaunch</depend>
<depend>robot_state_publisher</depend>
<depend>rviz</depend>
<depend>joint_state_publisher_gui</depend>
<depend>gazebo</depend>
<export>
<architecture_independent />
</export>
</package>

BIN
src/robots/go2_description/urdf/Amended_collision_model.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 213 KiB

BIN
src/robots/go2_description/urdf/Normal_collision_model.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 200 KiB

1217
src/robots/go2_description/urdf/go2_description.urdf Normal file
View File

File diff suppressed because it is too large Load Diff

121
src/robots/go2_description/xacro/const.xacro Executable file
View File

@ -0,0 +1,121 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- Constants for robot dimensions -->
<xacro:property name="PI" value="3.1415926535897931"/>
<xacro:property name="stick_mass" value="0.00001"/>
<!-- simplified collision value -->
<xacro:property name="trunk_width" value="0.0935"/>
<xacro:property name="trunk_length" value="0.3762"/>
<xacro:property name="trunk_height" value="0.114"/>
<xacro:property name="hip_radius" value="0.046"/>
<xacro:property name="hip_length" value="0.04"/>
<xacro:property name="thigh_shoulder_radius" value="0.041"/>
<xacro:property name="thigh_shoulder_length" value="0.032"/>
<xacro:property name="thigh_width" value="0.0245"/>
<xacro:property name="thigh_height" value="0.034"/>
<xacro:property name="calf_width" value="0.016"/>
<xacro:property name="calf_height" value="0.016"/>
<xacro:property name="foot_radius" value="0.02"/>
<xacro:property name="stick_radius" value="0.01"/>
<xacro:property name="stick_length" value="0.2"/>
<!-- kinematic value -->
<xacro:property name="thigh_offset" value="0.0955"/>
<xacro:property name="thigh_length" value="0.213"/>
<xacro:property name="calf_length" value="0.213"/>
<!-- leg offset from trunk center value -->
<xacro:property name="leg_offset_x" value="0.1934"/>
<xacro:property name="leg_offset_y" value="0.0465"/>
<!-- <xacro:property name="trunk_offset_z" value="0.01675"/> -->
<xacro:property name="hip_offset" value="0.0955"/>
<!-- offset of link locations (left front) -->
<xacro:property name="hip_offset_x" value="0.1934"/>
<xacro:property name="hip_offset_y" value="0.0465"/>
<xacro:property name="hip_offset_z" value="0.0"/>
<xacro:property name="thigh_offset_x" value="0"/>
<xacro:property name="thigh_offset_y" value="0.0955"/>
<xacro:property name="thigh_offset_z" value="0.0"/>
<xacro:property name="calf_offset_x" value="0.0"/>
<xacro:property name="calf_offset_y" value="0.0"/>
<xacro:property name="calf_offset_z" value="-0.213"/>
<!-- joint limits -->
<xacro:property name="damping" value="0.0"/>
<xacro:property name="friction" value="0.0"/>
<xacro:property name="hip_position_max" value="1.0472"/>
<xacro:property name="hip_position_min" value="-1.0472"/>
<xacro:property name="hip_velocity_max" value="30.1"/>
<xacro:property name="hip_torque_max" value="23.7"/>
<xacro:property name="thigh_position_max" value="3.4907"/>
<xacro:property name="thigh_position_min" value="-1.5708"/>
<xacro:property name="thigh_velocity_max" value="30.1"/>
<xacro:property name="thigh_torque_max" value="23.7"/>
<xacro:property name="calf_position_max" value="-0.83776"/>
<xacro:property name="calf_position_min" value="-2.7227"/>
<xacro:property name="calf_velocity_max" value="20.06"/>
<xacro:property name="calf_torque_max" value="35.55"/>
<!-- dynamics inertial value -->
<!-- trunk -->
<xacro:property name="trunk_mass" value="6.921"/>
<xacro:property name="trunk_com_x" value="0.021112"/>
<xacro:property name="trunk_com_y" value="0.00"/>
<xacro:property name="trunk_com_z" value="-0.005366"/>
<xacro:property name="trunk_ixx" value="0.02448"/>
<xacro:property name="trunk_ixy" value="0.00012166"/>
<xacro:property name="trunk_ixz" value="0.0014849"/>
<xacro:property name="trunk_iyy" value="0.098077"/>
<xacro:property name="trunk_iyz" value="-3.12E-05"/>
<xacro:property name="trunk_izz" value="0.107"/>
<!-- hip (left front) -->
<xacro:property name="hip_mass" value="0.678"/>
<xacro:property name="hip_com_x" value="-0.0054"/>
<xacro:property name="hip_com_y" value="0.00194"/>
<xacro:property name="hip_com_z" value="-0.000105"/>
<xacro:property name="hip_ixx" value="0.00048"/>
<xacro:property name="hip_ixy" value="-3.01E-06"/>
<xacro:property name="hip_ixz" value="1.11E-06"/>
<xacro:property name="hip_iyy" value="0.000884"/>
<xacro:property name="hip_iyz" value="-1.42E-06"/>
<xacro:property name="hip_izz" value="0.000596"/>
<!-- thigh -->
<xacro:property name="thigh_mass" value="1.152"/>
<xacro:property name="thigh_com_x" value="-0.00374"/>
<xacro:property name="thigh_com_y" value="-0.0223"/>
<xacro:property name="thigh_com_z" value="-0.0327"/>
<xacro:property name="thigh_ixx" value="0.00584"/>
<xacro:property name="thigh_ixy" value="8.72E-05"/>
<xacro:property name="thigh_ixz" value="-0.000289"/>
<xacro:property name="thigh_iyy" value="0.0058"/>
<xacro:property name="thigh_iyz" value="0.000808"/>
<xacro:property name="thigh_izz" value="0.00103"/>
<!-- calf -->
<xacro:property name="calf_mass" value="0.154"/>
<xacro:property name="calf_com_x" value="0.00548"/>
<xacro:property name="calf_com_y" value="-0.000975"/>
<xacro:property name="calf_com_z" value="-0.115"/>
<xacro:property name="calf_ixx" value="0.00108"/>
<xacro:property name="calf_ixy" value="3.4E-07"/>
<xacro:property name="calf_ixz" value="1.72E-05"/>
<xacro:property name="calf_iyy" value="0.0011"/>
<xacro:property name="calf_iyz" value="8.28E-06"/>
<xacro:property name="calf_izz" value="3.29E-05"/>
<!-- foot -->
<xacro:property name="foot_mass" value="0.06"/>
</robot>

257
src/robots/go2_description/xacro/gazebo.xacro Normal file
View File

@ -0,0 +1,257 @@
<?xml version="1.0"?>
<robot>
<!-- 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. -->
<!--gazebo>
<plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
<frequency>10</frequency>
<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. -->
<gazebo>
<plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
<frequency>100</frequency>
<plot>
<link>FL_foot</link>
<pose>0 0 0 0 0 0</pose>
</plot>
</plugin>
</gazebo>
<gazebo>
<plugin filename="libgazebo_ros_force.so" name="gazebo_ros_force">
<bodyName>trunk</bodyName>
<topicName>/apply_force/trunk</topicName>
</plugin>
</gazebo>
<gazebo reference="imu_link">
<gravity>true</gravity>
<sensor name="imu_sensor" type="imu">
<always_on>true</always_on>
<update_rate>1000</update_rate>
<visualize>true</visualize>
<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>
<!-- Foot contacts. -->
<gazebo reference="FR_calf">
<sensor name="FR_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
<contact>
<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. -->
<gazebo reference="FR_foot">
<visual>
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
<topicName>FR_foot_contact</topicName>
</plugin>
</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">
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="trunk">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="stick_link">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
</gazebo>
<gazebo reference="imu_link">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
</gazebo>
<!-- FL leg -->
<gazebo reference="FL_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
</gazebo>
<gazebo reference="FL_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="FL_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="FL_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<!-- FR leg -->
<gazebo reference="FR_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
</gazebo>
<gazebo reference="FR_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="FR_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="FR_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<!-- RL leg -->
<gazebo reference="RL_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
</gazebo>
<gazebo reference="RL_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="RL_calf">
<mu1>0.2</mu1>
<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>

176
src/robots/go2_description/xacro/leg.xacro Executable file
View File

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

40
src/robots/go2_description/xacro/materials.xacro Executable file
View File

@ -0,0 +1,40 @@
<?xml version="1.0"?>
<robot>
<material name="black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
<material name="blue">
<color rgba="0.0 0.0 0.8 1.0"/>
</material>
<material name="green">
<color rgba="0.0 0.8 0.0 1.0"/>
</material>
<material name="grey">
<color rgba="0.2 0.2 0.2 0.0"/>
</material>
<material name="silver">
<color rgba="${233/255} ${233/255} ${216/255} 1.0"/>
</material>
<material name="orange">
<color rgba="${255/255} ${108/255} ${10/255} 1.0"/>
</material>
<material name="brown">
<color rgba="${222/255} ${207/255} ${195/255} 1.0"/>
</material>
<material name="red">
<color rgba="0.8 0.0 0.0 1.0"/>
</material>
<material name="white">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
</robot>

125
src/robots/go2_description/xacro/robot.xacro Executable file
View File

@ -0,0 +1,125 @@
<?xml version="1.0"?>
<robot name="go2" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:arg name="DEBUG" default="false"/>
<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/leg.xacro"/>
<!-- <xacro:include filename="$(find go2_description)/xacro/stairs.xacro"/> -->
<xacro:include filename="$(find go2_description)/xacro/gazebo.xacro"/>
<!-- <xacro:include filename="$(find go2_gazebo)/launch/stairs.urdf.xacro"/> -->
<!-- <xacro:stairs stairs="15" xpos="0" ypos="0" zpos="0" /> -->
<!-- Rotor related joint and link is only for demonstrate location. -->
<!-- Actually, the rotor will rotate and the joint is not fixed. Reduction ratio should be considered. -->
<!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
<xacro:if value="$(arg DEBUG)">
<link name="world"/>
<joint name="base_static_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="world"/>
<child link="base"/>
</joint>
</xacro:if>
<link name="base">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</visual>
</link>
<joint name="floating_base" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="base"/>
<child link="trunk"/>
</joint>
<link name="trunk">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
</geometry>
<!-- <material name="orange"/> -->
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="${trunk_length} ${trunk_width} ${trunk_height}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${trunk_com_x} ${trunk_com_y} ${trunk_com_z}"/>
<mass value="${trunk_mass}"/>
<inertia
ixx="${trunk_ixx}" ixy="${trunk_ixy}" ixz="${trunk_ixz}"
iyy="${trunk_iyy}" iyz="${trunk_iyz}"
izz="${trunk_izz}"/>
</inertial>
</link>
<joint name="imu_joint" type="fixed">
<parent link="trunk"/>
<child link="imu_link"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
<link name="imu_link">
<inertial>
<mass value="0.001"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
<!-- <material name="red"/> -->
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size=".001 .001 .001"/>
</geometry>
</collision>
</link>
<!--
<joint name="load_joint" type="fixed">
<parent link="trunk"/>
<child link="load_link"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
<link name="load_link">
<inertial>
<mass value="5"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.2"/>
<geometry>
<box size="0.5 0.3 0.2"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size=".001 .001 .001"/>
</geometry>
</collision>
</link>
-->
<xacro:leg name="FR" mirror="-1" mirror_dae= "False" front_hind="1" front_hind_dae="True" />
<xacro:leg name="FL" mirror="1" mirror_dae="True" front_hind="1" front_hind_dae="True" />
<xacro:leg name="RR" mirror="-1" mirror_dae="False" front_hind="-1" front_hind_dae="False" />
<xacro:leg name="RL" mirror="1" mirror_dae="True" front_hind="-1" front_hind_dae="False" />
</robot>

42
src/robots/go2_description/xacro/transmission.xacro Executable file
View File

@ -0,0 +1,42 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="leg_transmission" params="name">
<transmission name="${name}_hip_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${name}_hip_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="${name}_hip_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${name}_thigh_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${name}_thigh_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="${name}_thigh_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${name}_calf_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${name}_calf_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="${name}_calf_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>
</robot>