diff --git a/robots/go2_description/CMakeLists.txt b/robots/go2_description/CMakeLists.txt
index a0cad9a4..200ea182 100644
--- a/robots/go2_description/CMakeLists.txt
+++ b/robots/go2_description/CMakeLists.txt
@@ -2,8 +2,9 @@ cmake_minimum_required(VERSION 2.8.3)
project(go2_description)
-find_package(catkin REQUIRED)
-
+find_package(catkin REQUIRED
+ velodyne_description
+)
catkin_package()
find_package(roslaunch)
diff --git a/robots/go2_description/package.xml b/robots/go2_description/package.xml
index ba52e138..f2738eb8 100644
--- a/robots/go2_description/package.xml
+++ b/robots/go2_description/package.xml
@@ -15,6 +15,7 @@ for go2_description robot
rviz
joint_state_publisher_gui
gazebo
+ velodyne_description
diff --git a/robots/go2_description/xacro/go2_generated.urdf b/robots/go2_description/xacro/go2_generated.urdf
new file mode 100644
index 00000000..2b539311
--- /dev/null
+++ b/robots/go2_description/xacro/go2_generated.urdf
@@ -0,0 +1,946 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ trunk
+ /apply_force/trunk
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ true
+ 30
+ 0 0 0 0 0 0
+ false
+
+
+
+ 1040
+ 1
+ 2.26892802759
+ -2.26892802759
+
+
+
+ 0.2
+ 30.0
+ 0.01
+
+
+ gaussian
+ 0.0
+ 0.004
+
+
+
+ scan
+ hokuyo_frame
+
+
+
+
+ true
+
+ true
+ 1000
+ true
+ __default_topic__
+
+ trunk_imu
+ imu_link
+ 1000.0
+ 0.0
+ 0 0 0
+ 0 0 0
+ imu_link
+
+ 0 0 0 0 0 0
+
+
+
+
+
+ 100
+
+
+ FR_calf_fixed_joint_lump__FR_foot_collision_1
+
+
+
+
+
+ 100
+
+
+ FL_calf_fixed_joint_lump__FL_foot_collision_1
+
+
+
+
+
+ 100
+
+
+ RR_calf_fixed_joint_lump__RR_foot_collision_1
+
+
+
+
+
+ 100
+
+
+ RL_calf_fixed_joint_lump__RL_foot_collision_1
+
+
+
+
+
+
+
+ FR_foot_contact
+
+
+
+
+
+
+ FL_foot_contact
+
+
+
+
+
+
+ RR_foot_contact
+
+
+
+
+
+
+ RL_foot_contact
+
+
+
+
+ false
+
+
+ 0.2
+ 0.2
+
+
+
+
+ 0.2
+ 0.2
+
+
+ 0.2
+ 0.2
+
+
+
+ 0.2
+ 0.2
+
+
+ 0.2
+ 0.2
+ 1
+
+
+
+
+ 0.2
+ 0.2
+ 1
+
+
+ 0.6
+ 0.6
+ 1
+
+
+
+
+
+ 0.2
+ 0.2
+
+
+ 0.2
+ 0.2
+ 1
+
+
+
+
+ 0.2
+ 0.2
+ 1
+
+
+ 0.6
+ 0.6
+ 1
+
+
+
+
+
+ 0.2
+ 0.2
+
+
+ 0.2
+ 0.2
+ 1
+
+
+
+
+ 0.2
+ 0.2
+ 1
+
+
+ 0.6
+ 0.6
+ 1
+
+
+
+
+
+ 0.2
+ 0.2
+
+
+ 0.2
+ 0.2
+ 1
+
+
+
+
+ 0.2
+ 0.2
+ 1
+
+
+ 0.6
+ 0.6
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ hardware_interface/EffortJointInterface
+ 1
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ hardware_interface/EffortJointInterface
+ 1
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ hardware_interface/EffortJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ hardware_interface/EffortJointInterface
+ 1
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ hardware_interface/EffortJointInterface
+ 1
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ hardware_interface/EffortJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ hardware_interface/EffortJointInterface
+ 1
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ hardware_interface/EffortJointInterface
+ 1
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ hardware_interface/EffortJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ hardware_interface/EffortJointInterface
+ 1
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ hardware_interface/EffortJointInterface
+ 1
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ hardware_interface/EffortJointInterface
+ 1
+
+
+
+
diff --git a/robots/go2_description/xacro/go2_velodyne_description.urdf b/robots/go2_description/xacro/go2_velodyne_description.urdf
new file mode 100644
index 00000000..238a6448
--- /dev/null
+++ b/robots/go2_description/xacro/go2_velodyne_description.urdf
@@ -0,0 +1,980 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ trunk
+ /apply_force/trunk
+
+
+
+ true
+
+ true
+ 1000
+ true
+ __default_topic__
+
+ trunk_imu
+ imu_link
+ 1000.0
+ 0.0
+ 0 0 0
+ 0 0 0
+ imu_link
+
+ 0 0 0 0 0 0
+
+
+
+
+
+ 100
+
+
+ FR_calf_fixed_joint_lump__FR_foot_collision_1
+
+
+
+
+
+ 100
+
+
+ FL_calf_fixed_joint_lump__FL_foot_collision_1
+
+
+
+
+
+ 100
+
+
+ RR_calf_fixed_joint_lump__RR_foot_collision_1
+
+
+
+
+
+ 100
+
+
+ RL_calf_fixed_joint_lump__RL_foot_collision_1
+
+
+
+
+
+
+
+ FR_foot_contact
+
+
+
+
+
+
+ FL_foot_contact
+
+
+
+
+
+
+ RR_foot_contact
+
+
+
+
+
+
+ RL_foot_contact
+
+
+
+
+ false
+
+
+ 0.2
+ 0.2
+
+
+
+
+ 0.2
+ 0.2
+
+
+ 0.2
+ 0.2
+
+
+
+ 0.2
+ 0.2
+
+
+ 0.2
+ 0.2
+ 1
+
+
+
+
+ 0.2
+ 0.2
+ 1
+
+
+ 0.6
+ 0.6
+ 1
+
+
+
+
+
+ 0.2
+ 0.2
+
+
+ 0.2
+ 0.2
+ 1
+
+
+
+
+ 0.2
+ 0.2
+ 1
+
+
+ 0.6
+ 0.6
+ 1
+
+
+
+
+
+ 0.2
+ 0.2
+
+
+ 0.2
+ 0.2
+ 1
+
+
+
+
+ 0.2
+ 0.2
+ 1
+
+
+ 0.6
+ 0.6
+ 1
+
+
+
+
+
+ 0.2
+ 0.2
+
+
+ 0.2
+ 0.2
+ 1
+
+
+
+
+ 0.2
+ 0.2
+ 1
+
+
+ 0.6
+ 0.6
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0 0 0 0 0 0
+ false
+ 10
+
+
+
+ 440
+ 1
+ -3.141592653589793
+ 3.141592653589793
+
+
+ 16
+ 1
+ -0.2617993877991494
+ 0.2617993877991494
+
+
+
+ 0.3
+ 131.0
+ 0.001
+
+
+ gaussian
+ 0.0
+ 0.0
+
+
+
+ /velodyne_points
+ velodyne
+ False
+ 0.9
+ 130.0
+ 0.008
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ hardware_interface/EffortJointInterface
+ 1
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ hardware_interface/EffortJointInterface
+ 1
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ hardware_interface/EffortJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ hardware_interface/EffortJointInterface
+ 1
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ hardware_interface/EffortJointInterface
+ 1
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ hardware_interface/EffortJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ hardware_interface/EffortJointInterface
+ 1
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ hardware_interface/EffortJointInterface
+ 1
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ hardware_interface/EffortJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ hardware_interface/EffortJointInterface
+ 1
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ hardware_interface/EffortJointInterface
+ 1
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ hardware_interface/EffortJointInterface
+ 1
+
+
+
diff --git a/robots/go2_description/xacro/robot.xacro b/robots/go2_description/xacro/robot.xacro
index c03da970..aa5a81e2 100755
--- a/robots/go2_description/xacro/robot.xacro
+++ b/robots/go2_description/xacro/robot.xacro
@@ -10,6 +10,12 @@
+
+
+
+
+
+
@@ -35,6 +41,11 @@
+
+
+
+
+