diff --git a/robots/b1_description/xacro/b1.xacro b/robots/b1_description/xacro/b1.xacro
new file mode 100644
index 00000000..d7daaa56
--- /dev/null
+++ b/robots/b1_description/xacro/b1.xacro
@@ -0,0 +1,94 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/robots/b1_description/xacro/robot.xacro b/robots/b1_description/xacro/robot.xacro
index 30b6127e..7b2be0bb 100644
--- a/robots/b1_description/xacro/robot.xacro
+++ b/robots/b1_description/xacro/robot.xacro
@@ -1,102 +1,27 @@
+
-
-
-
+
+
+
+
+
-
-
+
+
-
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
diff --git a/robots/b1z1_description/CMakeLists.txt b/robots/b1z1_description/CMakeLists.txt
new file mode 100644
index 00000000..143bb632
--- /dev/null
+++ b/robots/b1z1_description/CMakeLists.txt
@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(b1z1_description)
+
+find_package(catkin REQUIRED)
diff --git a/robots/b1z1_description/config/robot_control.yaml b/robots/b1z1_description/config/robot_control.yaml
new file mode 100644
index 00000000..26075320
--- /dev/null
+++ b/robots/b1z1_description/config/robot_control.yaml
@@ -0,0 +1,105 @@
+b1z1_gazebo:
+ # Publish all joint states -----------------------------------
+ joint_state_controller:
+ type: joint_state_controller/JointStateController
+ publish_rate: 1000
+
+ # FL Controllers ---------------------------------------
+ FL_hip_controller:
+ type: unitree_legged_control/UnitreeJointController
+ joint: FL_hip_joint
+ pid: {p: 100.0, i: 0.0, d: 5.0}
+
+ FL_thigh_controller:
+ type: unitree_legged_control/UnitreeJointController
+ joint: FL_thigh_joint
+ pid: {p: 300.0, i: 0.0, d: 8.0}
+
+ FL_calf_controller:
+ type: unitree_legged_control/UnitreeJointController
+ joint: FL_calf_joint
+ pid: {p: 300.0, i: 0.0, d: 8.0}
+
+ # FR Controllers ---------------------------------------
+ FR_hip_controller:
+ type: unitree_legged_control/UnitreeJointController
+ joint: FR_hip_joint
+ pid: {p: 100.0, i: 0.0, d: 5.0}
+
+ FR_thigh_controller:
+ type: unitree_legged_control/UnitreeJointController
+ joint: FR_thigh_joint
+ pid: {p: 300.0, i: 0.0, d: 8.0}
+
+ FR_calf_controller:
+ type: unitree_legged_control/UnitreeJointController
+ joint: FR_calf_joint
+ pid: {p: 300.0, i: 0.0, d: 8.0}
+
+ # RL Controllers ---------------------------------------
+ RL_hip_controller:
+ type: unitree_legged_control/UnitreeJointController
+ joint: RL_hip_joint
+ pid: {p: 100.0, i: 0.0, d: 5.0}
+
+ RL_thigh_controller:
+ type: unitree_legged_control/UnitreeJointController
+ joint: RL_thigh_joint
+ pid: {p: 300.0, i: 0.0, d: 8.0}
+
+ RL_calf_controller:
+ type: unitree_legged_control/UnitreeJointController
+ joint: RL_calf_joint
+ pid: {p: 300.0, i: 0.0, d: 8.0}
+
+ # RR Controllers ---------------------------------------
+ RR_hip_controller:
+ type: unitree_legged_control/UnitreeJointController
+ joint: RR_hip_joint
+ pid: {p: 100.0, i: 0.0, d: 5.0}
+
+ RR_thigh_controller:
+ type: unitree_legged_control/UnitreeJointController
+ joint: RR_thigh_joint
+ pid: {p: 300.0, i: 0.0, d: 8.0}
+
+ RR_calf_controller:
+ type: unitree_legged_control/UnitreeJointController
+ joint: RR_calf_joint
+ pid: {p: 300.0, i: 0.0, d: 8.0}
+
+ # Z1 Controllers ---------------------------------------
+ Joint01_controller:
+ type: unitree_legged_control/UnitreeJointController
+ joint: joint1
+ pid: {p: 300.0, i: 0.0, d: 5.0}
+
+ Joint02_controller:
+ type: unitree_legged_control/UnitreeJointController
+ joint: joint2
+ pid: {p: 300.0, i: 0.0, d: 5.0}
+
+ Joint03_controller:
+ type: unitree_legged_control/UnitreeJointController
+ joint: joint3
+ pid: {p: 300.0, i: 0.0, d: 5.0}
+
+ Joint04_controller:
+ type: unitree_legged_control/UnitreeJointController
+ joint: joint4
+ pid: {p: 300.0, i: 0.0, d: 5.0}
+
+ Joint05_controller:
+ type: unitree_legged_control/UnitreeJointController
+ joint: joint5
+ pid: {p: 300.0, i: 0.0, d: 5.0}
+
+ Joint06_controller:
+ type: unitree_legged_control/UnitreeJointController
+ joint: joint6
+ pid: {p: 300.0, i: 0.0, d: 5.0}
+
+ gripper_controller:
+ type: unitree_legged_control/UnitreeJointController
+ joint: jointGripper
+ pid: {p: 300.0, i: 0.0, d: 5.0}
diff --git a/robots/b1z1_description/launch/b1z1_gazebo.launch b/robots/b1z1_description/launch/b1z1_gazebo.launch
new file mode 100644
index 00000000..66071a37
--- /dev/null
+++ b/robots/b1z1_description/launch/b1z1_gazebo.launch
@@ -0,0 +1,73 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/robots/b1z1_description/launch/b1z1_rviz.launch b/robots/b1z1_description/launch/b1z1_rviz.launch
new file mode 100644
index 00000000..98ae5da3
--- /dev/null
+++ b/robots/b1z1_description/launch/b1z1_rviz.launch
@@ -0,0 +1,18 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/robots/b1z1_description/package.xml b/robots/b1z1_description/package.xml
new file mode 100644
index 00000000..183ac87c
--- /dev/null
+++ b/robots/b1z1_description/package.xml
@@ -0,0 +1,12 @@
+
+
+ b1z1_description
+ 0.0.0
+ The b1z1_description package
+
+ unitree
+ TODO
+
+ catkin
+
+
diff --git a/robots/b1z1_description/xacro/const.xacro b/robots/b1z1_description/xacro/const.xacro
new file mode 100644
index 00000000..b064cd4c
--- /dev/null
+++ b/robots/b1z1_description/xacro/const.xacro
@@ -0,0 +1,17 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/robots/b1z1_description/xacro/gazebo.xacro b/robots/b1z1_description/xacro/gazebo.xacro
new file mode 100644
index 00000000..4c18e01c
--- /dev/null
+++ b/robots/b1z1_description/xacro/gazebo.xacro
@@ -0,0 +1,282 @@
+
+
+
+
+
+
+
+
+ /b1z1_gazebo
+ gazebo_ros_control/DefaultRobotHWSim
+
+
+
+
+
+
+ 10
+
+ base
+ 0 0 0 0 0 0
+ Gazebo/Yellow
+
+
+
+
+
+
+
+
+
+ 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
+
+
+
+
+
+ Gazebo/Green
+ false
+
+
+
+ 0.2
+ 0.2
+
+
+
+
+
+ 0.2
+ 0.2
+ Gazebo/White
+
+
+
+ 0.2
+ 0.2
+ Gazebo/Red
+
+
+
+
+ 0.2
+ 0.2
+ Gazebo/DarkGrey
+
+
+ 0.2
+ 0.2
+ 1
+ Gazebo/DarkGrey
+
+
+
+
+ 0.2
+ 0.2
+ 1
+
+
+ 0.6
+ 0.6
+ 1
+ Gazebo/DarkGrey
+
+
+
+
+
+
+ 0.2
+ 0.2
+ Gazebo/DarkGrey
+
+
+ 0.2
+ 0.2
+ 1
+ Gazebo/DarkGrey
+
+
+
+
+ 0.2
+ 0.2
+ 1
+
+
+ 0.6
+ 0.6
+ 1
+ Gazebo/DarkGrey
+
+
+
+
+
+
+ 0.2
+ 0.2
+ Gazebo/DarkGrey
+
+
+ 0.2
+ 0.2
+ 1
+ Gazebo/DarkGrey
+
+
+
+
+ 0.2
+ 0.2
+ 1
+
+
+ 0.6
+ 0.6
+ 1
+ Gazebo/DarkGrey
+
+
+
+
+
+
+ 0.2
+ 0.2
+ Gazebo/DarkGrey
+
+
+ 0.2
+ 0.2
+ 1
+ Gazebo/DarkGrey
+
+
+
+
+ 0.2
+ 0.2
+ 1
+
+
+ 0.6
+ 0.6
+ 1
+ Gazebo/DarkGrey
+
+
+
+
+
+
+ true
+
+
+
+ true
+
+
+
+ true
+
+
+
diff --git a/robots/b1z1_description/xacro/robot.xacro b/robots/b1z1_description/xacro/robot.xacro
new file mode 100644
index 00000000..b42f95b3
--- /dev/null
+++ b/robots/b1z1_description/xacro/robot.xacro
@@ -0,0 +1,43 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/robots/z1_description/xacro/robot.xacro b/robots/z1_description/xacro/robot.xacro
index 7f57a05a..ac18d224 100644
--- a/robots/z1_description/xacro/robot.xacro
+++ b/robots/z1_description/xacro/robot.xacro
@@ -1,316 +1,26 @@
+
-
+
+
+
+
+
-
-
+
+
+
-
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- true
-
-
-
- true
-
-
-
-
-
-
-
-
-
-
diff --git a/robots/z1_description/xacro/z1.xacro b/robots/z1_description/xacro/z1.xacro
new file mode 100644
index 00000000..7b61b880
--- /dev/null
+++ b/robots/z1_description/xacro/z1.xacro
@@ -0,0 +1,310 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ true
+
+
+
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+