diff --git a/README.md b/README.md
index d09e0d7..bea0c89 100644
--- a/README.md
+++ b/README.md
@@ -41,4 +41,4 @@ ros2 launch go2_description unitree_guide.launch.py
 source ~/ros2_ws/install/setup.bash
 ros2 run keyboard_input keyboard_input
 ```
-For more details, please refer to the [unitree guide controller](controllers/unitree_guide_controller/) and [go2 description](descriptions/go2_description/).
\ No newline at end of file
+For more details, please refer to the [unitree guide controller](controllers/unitree_guide_controller/) and [go2 description](descriptions/unitree/go2_description/).
\ No newline at end of file
diff --git a/controllers/unitree_guide_controller/README.md b/controllers/unitree_guide_controller/README.md
index 05aac0a..a1c1566 100644
--- a/controllers/unitree_guide_controller/README.md
+++ b/controllers/unitree_guide_controller/README.md
@@ -38,5 +38,5 @@ colcon build --packages-up-to unitree_guide_controller
 ```
 
 ## 3. Run
-* [Go2 in mujoco simulation](../../descriptions/go2_description)
+* [Go2 in mujoco simulation](../../descriptions/unitree/go2_description)
 * [Go1/A1 in gazebo simulation](../../descriptions/quadruped_gazebo)
\ No newline at end of file
diff --git a/descriptions/README.md b/descriptions/README.md
new file mode 100644
index 0000000..9989330
--- /dev/null
+++ b/descriptions/README.md
@@ -0,0 +1,25 @@
+# Robot Descriptions
+
+This folder contains the URDF and SRDF files for the quadruped robot. 
+
+* Unitree
+  * [Go1](unitree/go1_description/)
+  * [Go2](unitree/go2_description/)
+  * [Aliengo](unitree/aliengo_description/)
+  * [B2](unitree/b2_description/)
+* Xiaomi
+  * [Cyberdog](xiaomi/cyberdog_description/)
+
+
+## Steps to transfer urdf to Mujoco model
+* Install [Mujoco](https://github.com/google-deepmind/mujoco)
+* 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.
+* use `xacro` to generate the urdf file.
+  ```
+  xacro robot.xacro > ../urdf/robot.urdf
+  ```
+* use mujoco to convert the urdf file to mujoco model.
+  ```
+  compile robot.urdf robot.xml
+  ```
\ No newline at end of file
diff --git a/descriptions/cyberdog_description/config/robot_control.yaml b/descriptions/cyberdog_description/config/robot_control.yaml
deleted file mode 100644
index 8807c0f..0000000
--- a/descriptions/cyberdog_description/config/robot_control.yaml
+++ /dev/null
@@ -1,95 +0,0 @@
-# Controller Manager configuration
-controller_manager:
-  ros__parameters:
-    update_rate: 500  # 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
-
-imu_sensor_broadcaster:
-  ros__parameters:
-    sensor_name: "imu_sensor"
-    frame_id: "imu_link"
-
-leg_pd_controller:
-  ros__parameters:
-    joints:
-      - FR_abad_joint
-      - FR_hip_joint
-      - FR_knee_joint
-      - FL_abad_joint
-      - FL_hip_joint
-      - FL_knee_joint
-      - RR_abad_joint
-      - RR_hip_joint
-      - RR_knee_joint
-      - RL_abad_joint
-      - RL_hip_joint
-      - RL_knee_joint
-
-    command_interfaces:
-      - effort
-
-    state_interfaces:
-      - position
-      - velocity
-
-unitree_guide_controller:
-  ros__parameters:
-    command_prefix: "leg_pd_controller"
-    joints:
-      - FR_abad_joint
-      - FR_hip_joint
-      - FR_knee_joint
-      - FL_abad_joint
-      - FL_hip_joint
-      - FL_knee_joint
-      - RR_abad_joint
-      - RR_hip_joint
-      - RR_knee_joint
-      - RL_abad_joint
-      - RL_hip_joint
-      - RL_knee_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_link"
-
-    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
\ No newline at end of file
diff --git a/descriptions/cyberdog_description/xacro/const.xacro b/descriptions/cyberdog_description/xacro/const.xacro
deleted file mode 100644
index 87abb83..0000000
--- a/descriptions/cyberdog_description/xacro/const.xacro
+++ /dev/null
@@ -1,125 +0,0 @@
-<?xml version="1.0"?>
-
-<robot name="cyberdog" xmlns:xacro="http://www.ros.org/wiki/xacro">
-
-    <xacro:property name="PI" value="3.1415926535897931" />
-
-    <!-- dimension -->
-    <!-- body -->
-    <xacro:property name="body_width" value="0.190" />
-    <xacro:property name="body_length" value="0.236" />
-    <xacro:property name="body_height" value="0.109" />
-
-    <!-- abad -->
-    <xacro:property name="abad_radius" value="0.039" />
-    <xacro:property name="abad_length" value="0.035" />
-    <xacro:property name="abad_offset_x" value="0.164" />
-    <xacro:property name="abad_offset_y" value="0.042" />
-    <xacro:property name="abad_rotor_offset" value="0.0642303" />
-    <xacro:property name="abad_max" value="39" />
-    <xacro:property name="abad_min" value="-39" />
-
-    <!-- hip -->
-    <xacro:property name="hip_shoulder_radius" value="0.039" />
-    <xacro:property name="hip_shoulder_length" value="0.035" />
-    <xacro:property name="hip_width" value="0.0250" />
-    <xacro:property name="hip_height" value="0.034" />
-    <xacro:property name="hip_offset" value="0.094" />
-    <xacro:property name="hip_length" value="0.12" />
-    <xacro:property name="hip_rotor_offset" value="-0.07577" />
-    <xacro:property name="hip_f_max" value="160" />
-    <xacro:property name="hip_f_min" value="-76" />
-    <xacro:property name="hip_h_max" value="180" />
-    <xacro:property name="hip_h_min" value="-56" />
-
-    <!-- knee -->
-    <xacro:property name="knee_width" value="0.016" />
-    <xacro:property name="knee_height" value="0.016" />
-    <xacro:property name="knee_length" value="0.18341" />
-    <xacro:property name="knee_rotor_offset" value="-0.0342303" />
-    <xacro:property name="knee_max" value="-30" />
-    <xacro:property name="knee_min" value="-145" />
-    <xacro:property name="knee_rubber" value="0.02" />
-
-    <!-- foot -->
-    <xacro:property name="foot_radius" value="0.019" />
-
-    <!-- acuator atribute -->
-    <xacro:property name="abadGearRatio" value="7.75" />
-    <xacro:property name="hipGearRatio" value="7.75" />
-    <xacro:property name="kneeGearRatio" value="7.75" />
-
-    <xacro:property name="abad_motorTauMax" value="1.5484" />
-    <xacro:property name="hip_motorTauMax" value="1.5484" />
-    <xacro:property name="knee_motorTauMax" value="1.5484" />
-    <xacro:property name="abad_motorVelMax" value="30.9971" />
-    <xacro:property name="hip_motorVelMax" value="30.9971" />
-    <xacro:property name="knee_motorVelMax" value="30.9971" />
-    <xacro:property name="abad_damping" value="0.01" />
-    <xacro:property name="hip_damping" value="0.01" />
-    <xacro:property name="knee_damping" value="0.01" />
-    <xacro:property name="abad_friction" value="0.1" />
-    <xacro:property name="hip_friction" value="0.1" />
-    <xacro:property name="knee_friction" value="0.1" />
-
-    <!-- inertial&mass value -->
-    <!-- body -->
-    <xacro:property name="body_mass" value="4.030000000" />
-    <xacro:property name="body_com_x" value="0.027300000" />
-    <xacro:property name="body_com_y" value="-0.000242000" />
-    <xacro:property name="body_com_z" value="0.014300000" />
-    <xacro:property name="body_ixx" value="0.018500000" />
-    <xacro:property name="body_ixy" value="-0.000173000" />
-    <xacro:property name="body_ixz" value="-0.010200000" />
-    <xacro:property name="body_iyy" value="0.051700000" />
-    <xacro:property name="body_iyz" value="-0.000028300" />
-    <xacro:property name="body_izz" value="0.048300000" />
-
-    <!-- abad -->
-    <xacro:property name="abad_mass" value="0.354000000" />
-    <xacro:property name="abad_com_x" value="-0.003920000" />
-    <xacro:property name="abad_com_y" value="0.015000000" />
-    <xacro:property name="abad_com_z" value="-0.000306000" />
-    <xacro:property name="abad_ixx" value="0.000190000" />
-    <xacro:property name="abad_ixy" value="-0.000027000" />
-    <xacro:property name="abad_ixz" value="-0.000000344" />
-    <xacro:property name="abad_iyy" value="0.000276000" />
-    <xacro:property name="abad_iyz" value="0.000001950" />
-    <xacro:property name="abad_izz" value="0.000233000" />
-
-    <!-- hip -->
-    <xacro:property name="hip_mass" value="0.482000000" />
-    <xacro:property name="hip_com_x" value="-0.002120000" />
-    <xacro:property name="hip_com_y" value="-0.021200000" />
-    <xacro:property name="hip_com_z" value="-0.018400000" />
-    <xacro:property name="hip_ixx" value="0.001010000" />
-    <xacro:property name="hip_ixy" value="0.000022300" />
-    <xacro:property name="hip_ixz" value="-0.000038500" />
-    <xacro:property name="hip_iyy" value="0.000983000" />
-    <xacro:property name="hip_iyz" value="0.000199000" />
-    <xacro:property name="hip_izz" value="0.000347000" />
-
-    <!-- knee -->
-    <xacro:property name="knee_mass" value="0.116000000" />
-    <xacro:property name="knee_com_x" value="0.000600000" />
-    <xacro:property name="knee_com_y" value="-0.000047200" />
-    <xacro:property name="knee_com_z" value="-0.089300000" />
-    <xacro:property name="knee_ixx" value="0.000668000" />
-    <xacro:property name="knee_ixy" value="0.000000003" />
-    <xacro:property name="knee_ixz" value="0.000023700" />
-    <xacro:property name="knee_iyy" value="0.000674000" />
-    <xacro:property name="knee_iyz" value="0.000000603" />
-    <xacro:property name="knee_izz" value="0.000015400" />
-
-    <!-- rotor -->
-    <xacro:property name="rotor_mass" value="0.056700000" />
-    <xacro:property name="rotor_com_x" value="0" />
-    <xacro:property name="rotor_com_y" value="0" />
-    <xacro:property name="rotor_com_z" value="0" />
-    <xacro:property name="rotor_ixx" value="0.000025300" />
-    <xacro:property name="rotor_ixy" value="0" />
-    <xacro:property name="rotor_ixz" value="0" />
-    <xacro:property name="rotor_iyy" value="0.000047800" />
-    <xacro:property name="rotor_iyz" value="0" />
-    <xacro:property name="rotor_izz" value="0.000025300" />
-</robot>
\ No newline at end of file
diff --git a/descriptions/cyberdog_description/xacro/gazebo.xacro b/descriptions/cyberdog_description/xacro/gazebo.xacro
deleted file mode 100644
index 9f2653c..0000000
--- a/descriptions/cyberdog_description/xacro/gazebo.xacro
+++ /dev/null
@@ -1,154 +0,0 @@
-<?xml version="1.0"?>
-<robot name="cyber_dog" xmlns:xacro="http://www.ros.org/wiki/xacro">
-<!--    <gazebo>-->
-<!--        <plugin name="gazebo_rt_control" filename="liblegged_plugin.so">-->
-<!--        </plugin>-->
-<!--        <plugin name="gazebo_rt_control" filename="libreal_time_control.so">-->
-<!--            <robotName>$(arg ROBOT)</robotName>-->
-<!--        </plugin>-->
-<!--    </gazebo>-->
-
-    <gazebo reference="imu_link">
-        <sensor name="imu_sensor" type="imu">
-            <always_on>1</always_on>
-            <update_rate>500</update_rate>
-            <visualize>true</visualize>
-            <topic>imu</topic>
-            <imu>
-                <angular_velocity>
-                    <x>
-                        <noise type="gaussian">
-                            <mean>0.0</mean>
-                            <stddev>2e-4</stddev>
-                            <bias_mean>0.0000075</bias_mean>
-                            <bias_stddev>0.0000008</bias_stddev>
-                        </noise>
-                    </x>
-                    <y>
-                        <noise type="gaussian">
-                            <mean>0.0</mean>
-                            <stddev>2e-4</stddev>
-                            <bias_mean>0.0000075</bias_mean>
-                            <bias_stddev>0.0000008</bias_stddev>
-                        </noise>
-                    </y>
-                    <z>
-                        <noise type="gaussian">
-                            <mean>0.0</mean>
-                            <stddev>2e-4</stddev>
-                            <bias_mean>0.0000075</bias_mean>
-                            <bias_stddev>0.0000008</bias_stddev>
-                        </noise>
-                    </z>
-                </angular_velocity>
-                <linear_acceleration>
-                    <x>
-                        <noise type="gaussian">
-                            <mean>0.0</mean>
-                            <stddev>1.7e-2</stddev>
-                            <bias_mean>0.1</bias_mean>
-                            <bias_stddev>0.001</bias_stddev>
-                        </noise>
-                    </x>
-                    <y>
-                        <noise type="gaussian">
-                            <mean>0.0</mean>
-                            <stddev>1.7e-2</stddev>
-                            <bias_mean>0.1</bias_mean>
-                            <bias_stddev>0.001</bias_stddev>
-                        </noise>
-                    </y>
-                    <z>
-                        <noise type="gaussian">
-                            <mean>0.0</mean>
-                            <stddev>1.7e-2</stddev>
-                            <bias_mean>0.1</bias_mean>
-                            <bias_stddev>0.001</bias_stddev>
-                        </noise>
-                    </z>
-                </linear_acceleration>
-            </imu>
-        </sensor>
-    </gazebo>
-    <gazebo reference="imu_joint">
-        <disableFixedJointLumping>true</disableFixedJointLumping>
-    </gazebo>
-
-
-<!--    <xacro:if value="$(arg USE_LIDAR)">-->
-<!--        <gazebo reference="lidar_link">-->
-<!--        <sensor name="realsense" type="ray">-->
-<!--            <always_on>true</always_on>-->
-<!--            <visualize>true</visualize>-->
-<!--            <pose>0.0 0 0.0 0 0 0</pose>-->
-<!--            <update_rate>5</update_rate>-->
-<!--            <ray>-->
-<!--            <scan>-->
-<!--                <horizontal>-->
-<!--                <samples>180</samples>-->
-<!--                <resolution>1.000000</resolution>-->
-<!--                <min_angle>-1.5700</min_angle>-->
-<!--                <max_angle>1.5700</max_angle>-->
-<!--                </horizontal>-->
-<!--            </scan>-->
-<!--            <range>-->
-<!--                <min>0.01</min>-->
-<!--                <max>12.00</max>-->
-<!--                <resolution>0.015000</resolution>-->
-<!--            </range>-->
-<!--            <noise>-->
-<!--                <type>gaussian</type>-->
-<!--                <mean>0.0</mean>-->
-<!--                <stddev>0.01</stddev>-->
-<!--            </noise>-->
-<!--            </ray>-->
-<!--            <plugin name="cyberdog_laserscan" filename="libgazebo_ros_ray_sensor.so">-->
-<!--            <ros>-->
-<!--                <remapping>~/out:=scan</remapping>-->
-<!--            </ros>-->
-<!--            <output_type>sensor_msgs/LaserScan</output_type>-->
-<!--            <frame_name>lidar_link</frame_name>-->
-<!--            </plugin>-->
-<!--        </sensor>-->
-<!--        </gazebo>-->
-<!--    </xacro:if>-->
-
-<!--    &lt;!&ndash; Foot contacts. &ndash;&gt;-->
-<!--    <gazebo reference="FR_knee">-->
-<!--        <sensor name="FR_foot_contact" type="contact">-->
-<!--        <update_rate>100</update_rate>-->
-<!--        <plugin name="contactPlugin" filename="libfoot_contact_plugin.so"/>-->
-<!--        <contact>-->
-<!--            <collision>FR_knee_fixed_joint_lump__FR_foot_collision_2</collision>-->
-<!--        </contact>-->
-<!--        </sensor>-->
-<!--    </gazebo>-->
-<!--    <gazebo reference="FL_knee">-->
-<!--        <sensor name="FL_foot_contact" type="contact">-->
-<!--        <update_rate>100</update_rate>-->
-<!--        <plugin name="contactPlugin" filename="libfoot_contact_plugin.so"/>-->
-<!--        <contact>-->
-<!--            <collision>FL_knee_fixed_joint_lump__FL_foot_collision_2</collision>-->
-<!--        </contact>-->
-<!--        </sensor>-->
-<!--    </gazebo>-->
-<!--    <gazebo reference="RR_knee">-->
-<!--        <sensor name="RR_foot_contact" type="contact">-->
-<!--        <update_rate>100</update_rate>-->
-<!--        <plugin name="contactPlugin" filename="libfoot_contact_plugin.so"/>-->
-<!--        <contact>-->
-<!--            <collision>RR_knee_fixed_joint_lump__RR_foot_collision_2</collision>-->
-<!--        </contact>-->
-<!--        </sensor>-->
-<!--    </gazebo>-->
-<!--    <gazebo reference="RL_knee">-->
-<!--        <sensor name="RL_foot_contact" type="contact">-->
-<!--        <update_rate>100</update_rate>-->
-<!--        <plugin name="contactPlugin" filename="libfoot_contact_plugin.so"/>-->
-<!--        <contact>-->
-<!--            <collision>RL_knee_fixed_joint_lump__RL_foot_collision_2</collision>-->
-<!--        </contact>-->
-<!--        </sensor>-->
-<!--    </gazebo>-->
-
-</robot>
diff --git a/descriptions/cyberdog_description/xacro/leg.xacro b/descriptions/cyberdog_description/xacro/leg.xacro
deleted file mode 100644
index c732c33..0000000
--- a/descriptions/cyberdog_description/xacro/leg.xacro
+++ /dev/null
@@ -1,295 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
-
-    <xacro:include filename="$(find cyberdog_description)/xacro/transmission.xacro"/>
-
-    <xacro:macro name="leg" params="name mirror mirror_dae front_hind front_hind_dae *origin">
-
-        <joint name="${name}_abad_joint" type="revolute">
-            <xacro:insert_block name="origin"/>
-            <parent link="body"/>
-            <child link="${name}_abad"/>
-            <axis xyz="1 0 0"/>
-            <dynamics damping="${abad_damping}" friction="${abad_friction}"/>
-            <xacro:if value="${(mirror_dae == True)}">
-                <limit effort="${abad_motorTauMax*abadGearRatio}"
-                       velocity="${abad_motorVelMax}" lower="${abad_min*PI/180.0}"
-                       upper="${abad_max*PI/180.0}"/>
-            </xacro:if>
-            <xacro:if value="${(mirror_dae == False)}">
-                <limit effort="${abad_motorTauMax*abadGearRatio}"
-                       velocity="${abad_motorVelMax}" lower="${-abad_max*PI/180.0}"
-                       upper="${-abad_min*PI/180.0}"/>
-            </xacro:if>
-        </joint>
-
-        <link name="${name}_abad">
-            <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="file://$(find cyberdog_description)/meshes/abad.dae" scale="1 1 1"/>
-                </geometry>
-            </visual>
-            <collision>
-                <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
-                <geometry>
-                    <cylinder length="${abad_length}" radius="${abad_radius}"/>
-                </geometry>
-            </collision>
-            <inertial>
-                <origin rpy="0 0 0"
-                        xyz="${abad_com_x*front_hind} ${abad_com_y*mirror} ${abad_com_z}"/>
-                <mass value="${abad_mass}"/>
-                <inertia ixx="${abad_ixx}" ixy="${abad_ixy*mirror*front_hind}"
-                         ixz="${abad_ixz*front_hind}" iyy="${abad_iyy}" iyz="${abad_iyz*mirror}"
-                         izz="${abad_izz}"/>
-            </inertial>
-        </link>
-
-        <joint name="${name}_abad_rotor_fix" type="fixed">
-            <origin rpy="0 0 0" xyz="${-abad_rotor_offset*front_hind} 0 0"/>
-            <parent link="${name}_abad"/>
-            <child link="${name}_abad_rotor"/>
-        </joint>
-
-        <!-- this link is only for abad rotor inertial -->
-        <link name="${name}_abad_rotor">
-            <inertial>
-                <origin rpy="0 0 0" xyz="${rotor_com_x} ${rotor_com_y} ${rotor_com_z}"/>
-                <mass value="${rotor_mass}"/>
-                <inertia ixx="${rotor_ixx}" ixy="${rotor_ixy}" ixz="${rotor_ixz}" iyy="${rotor_iyy}" iyz="${rotor_iyz}"
-                         izz="${rotor_izz}"/>
-            </inertial>
-        </link>
-
-        <joint name="${name}_abad_fixed" type="fixed">
-            <origin rpy="0 0 0" xyz="0 ${(hip_offset-hip_shoulder_length)*mirror} 0"/>
-            <parent link="${name}_abad"/>
-            <child link="${name}_hip_shoulder"/>
-        </joint>
-
-        <link name="${name}_hip_shoulder">
-            <collision>
-                <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
-                <geometry>
-                    <cylinder length="${hip_shoulder_length}" radius="${hip_shoulder_radius}"/>
-                </geometry>
-            </collision>
-        </link>
-
-        <joint name="${name}_hip_joint" type="revolute">
-            <origin rpy="0 0 0" xyz="0 ${hip_offset*mirror} 0"/>
-            <parent link="${name}_abad"/>
-            <child link="${name}_hip"/>
-            <axis xyz="0 1 0"/>
-            <dynamics damping="${hip_damping}" friction="${hip_friction}"/>
-            <xacro:if value="${front_hind_dae == True}">
-                <limit effort="${hip_motorTauMax*hipGearRatio}"
-                       velocity="${hip_motorVelMax}" lower="${hip_f_min*PI/180.0}"
-                       upper="${hip_f_max*PI/180.0}"/>
-            </xacro:if>
-            <xacro:if value="${front_hind_dae == False}">
-                <limit effort="${hip_motorTauMax*hipGearRatio}"
-                       velocity="${hip_motorVelMax}" lower="${hip_h_min*PI/180.0}"
-                       upper="${hip_h_max*PI/180.0}"/>
-            </xacro:if>
-        </joint>
-
-        <link name="${name}_hip">
-            <visual>
-                <origin rpy="0 0 0" xyz="0 0 0"/>
-                <geometry>
-                    <xacro:if value="${mirror_dae == True}">
-                        <mesh filename="file://$(find cyberdog_description)/meshes/hip.dae"
-                              scale="1 1 1"/>
-                    </xacro:if>
-                    <xacro:if value="${mirror_dae == False}">
-                        <mesh filename="file://$(find cyberdog_description)/meshes/hip_mirror.dae"
-                              scale="1 1 1"/>
-                    </xacro:if>
-                </geometry>
-            </visual>
-            <collision>
-                <origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-hip_length/2.0}"/>
-                <geometry>
-                    <box size="${hip_length} ${hip_width} ${hip_height}"/>
-                </geometry>
-            </collision>
-            <inertial>
-                <origin rpy="0 0 0" xyz="${hip_com_x} ${hip_com_y*mirror} ${hip_com_z}"/>
-                <mass value="${hip_mass}"/>
-                <inertia ixx="${hip_ixx}" ixy="${hip_ixy*mirror}" ixz="${hip_ixz}" iyy="${hip_iyy}"
-                         iyz="${hip_iyz*mirror}" izz="${hip_izz}"/>
-            </inertial>
-        </link>
-
-        <joint name="${name}_hip_rotor_fix" type="fixed">
-            <origin rpy="0 0 0" xyz="0 ${hip_rotor_offset*mirror} 0"/>
-            <parent link="${name}_hip"/>
-            <child link="${name}_hip_rotor"/>
-        </joint>
-
-        <!-- this link is only for hip rotor inertial -->
-        <link name="${name}_hip_rotor">
-            <inertial>
-                <origin rpy="0 0 0" xyz="${rotor_com_x} ${rotor_com_y} ${rotor_com_z}"/>
-                <mass value="${rotor_mass}"/>
-                <inertia ixx="${rotor_ixx}" ixy="${rotor_ixy}" ixz="${rotor_ixz}" iyy="${rotor_iyy}" iyz="${rotor_iyz}"
-                         izz="${rotor_izz}"/>
-            </inertial>
-        </link>
-
-        <joint name="${name}_knee_rotor_fix" type="fixed">
-            <origin rpy="0 0 0" xyz="0 ${knee_rotor_offset*mirror} 0"/>
-            <parent link="${name}_hip"/>
-            <child link="${name}_knee_rotor"/>
-        </joint>
-
-        <!-- this link is only for knee rotor inertial -->
-        <link name="${name}_knee_rotor">
-            <inertial>
-                <origin rpy="0 0 0" xyz="${rotor_com_x} ${rotor_com_y} ${rotor_com_z}"/>
-                <mass value="${rotor_mass}"/>
-                <inertia ixx="${rotor_ixx}" ixy="${rotor_ixy}" ixz="${rotor_ixz}" iyy="${rotor_iyy}" iyz="${rotor_iyz}"
-                         izz="${rotor_izz}"/>
-            </inertial>
-        </link>
-
-        <joint name="${name}_knee_joint" type="revolute">
-            <origin rpy="0 0 0" xyz="0 0 ${-hip_length}"/>
-            <parent link="${name}_hip"/>
-            <child link="${name}_knee"/>
-            <axis xyz="0 1 0"/>
-            <dynamics damping="${hip_damping}" friction="${hip_friction}"/>
-            <limit effort="${knee_motorTauMax*kneeGearRatio}"
-                   velocity="${knee_motorVelMax}" lower="${knee_min*PI/180.0}"
-                   upper="${knee_max*PI/180.0}"/>
-        </joint>
-
-        <link name="${name}_knee">
-            <visual>
-                <origin rpy="0 0 0" xyz="0 0 0"/>
-                <geometry>
-                    <mesh filename="file://$(find cyberdog_description)/meshes/knee.dae" scale="1 1 1"/>
-                </geometry>
-            </visual>
-            <collision>
-                <origin rpy="0 ${PI} 0" xyz="0 0 ${-knee_length/2.0}"/>
-                <geometry>
-                    <box size="${knee_height} ${knee_width} ${knee_length}"/>
-                </geometry>
-            </collision>
-            <collision name="${name}_knee_rubber">
-                <origin rpy="0 ${PI} 0" xyz="${-knee_rubber/2.0} 0 -0.007"/>
-                <geometry>
-                    <sphere radius="0.016"/>
-                </geometry>
-                <surface>
-                    <contact>
-                        <ode>
-                            <max_vel>0.00001</max_vel>
-                            <min_depth>0.0</min_depth>
-                        </ode>
-                    </contact>
-                </surface>
-            </collision>
-            <inertial>
-                <origin rpy="0 0 0" xyz="${knee_com_x} ${knee_com_y} ${knee_com_z}"/>
-                <mass value="${knee_mass}"/>
-                <inertia ixx="${knee_ixx}" ixy="${knee_ixy}" ixz="${knee_ixz}" iyy="${knee_iyy}"
-                         iyz="${knee_iyz}" izz="${knee_izz}"/>
-            </inertial>
-        </link>
-
-        <joint name="${name}_foot_fixed" type="fixed">
-            <origin rpy="0 0 0" xyz="0.0055 0 ${-knee_length+foot_radius/2.0}"/>
-            <parent link="${name}_knee"/>
-            <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>
-            </visual>
-            <collision>
-                <origin rpy="0 0 0" xyz="0 0 0"/>
-                <geometry>
-                    <sphere radius="${foot_radius}"/>
-                </geometry>
-                <surface>
-                    <contact>
-                        <ode>
-                            <max_vel>0.001</max_vel>
-                            <min_depth>0.0</min_depth>
-                        </ode>
-                    </contact>
-                </surface>
-            </collision>
-        </link>
-
-        <xacro:leg_transmission name="${name}"/>
-
-        <gazebo reference="${name}_abad">
-            <mu1>0.2</mu1>
-            <mu2>0.2</mu2>
-            <visual>
-                <material>
-                    <ambient>.5 .5 .5 1.0</ambient>
-                    <diffuse>.5 .5 .5 1.0</diffuse>
-                    <specular>.5 .5 .5 1.0</specular>
-                </material>
-            </visual>
-        </gazebo>
-        <gazebo reference="${name}_hip">
-            <mu1>0.2</mu1>
-            <mu2>0.2</mu2>
-            <self_collide>1</self_collide>
-            <visual>
-                <material>
-                    <ambient>.05 .05 .05 1.0</ambient>
-                    <diffuse>.05 .05 .05 1.0</diffuse>
-                    <specular>.05 .05 .05 1.0</specular>
-                </material>
-            </visual>
-        </gazebo>
-        <gazebo reference="${name}_knee">
-            <mu1>0.7</mu1>
-            <mu2>0.7</mu2>
-            <self_collide>1</self_collide>
-            <visual>
-                <material>
-                    <ambient>.5 .5 .5 1.0</ambient>
-                    <diffuse>.5 .5 .5 1.0</diffuse>
-                    <specular>.5 .5 .5 1.0</specular>
-                </material>
-            </visual>
-        </gazebo>
-        <gazebo reference="${name}_foot">
-            <mu1>0.7</mu1>
-            <mu2>0.7</mu2>
-            <self_collide>1</self_collide>
-            <visual>
-                <material>
-                    <ambient>.5 .5 .5 1.0</ambient>
-                    <diffuse>.5 .5 .5 1.0</diffuse>
-                    <specular>.5 .5 .5 1.0</specular>
-                </material>
-            </visual>
-        </gazebo>
-
-    </xacro:macro>
-</robot>
\ No newline at end of file
diff --git a/descriptions/cyberdog_description/xacro/robot.xacro b/descriptions/cyberdog_description/xacro/robot.xacro
deleted file mode 100644
index 8c194a1..0000000
--- a/descriptions/cyberdog_description/xacro/robot.xacro
+++ /dev/null
@@ -1,212 +0,0 @@
-<?xml version="1.0"?>
-<robot name="cyber_dog" xmlns:xacro="http://www.ros.org/wiki/xacro">
-
-    <xacro:arg name="ROBOT" default="cyber_dog"/>
-    <xacro:arg name="USE_LIDAR" default="false"/>
-    <xacro:include filename="const.xacro"/>
-    <xacro:include filename="leg.xacro"/>
-    <xacro:include filename="gazebo.xacro"/>
-    <xacro:include filename="ros2_control.xacro"/>
-
-    <link name="base_link">
-        <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_link"/>
-        <child link="body"/>
-    </joint>
-
-    <link name="body">
-        <visual>
-            <origin rpy="0 0 0" xyz="0 0 0"/>
-            <geometry>
-                <mesh filename="file://$(find cyberdog_description)/meshes/body.dae" scale="1 1 1"/>
-            </geometry>
-        </visual>
-        <collision>
-            <origin rpy="0 0 0" xyz="0 0 0"/>
-            <geometry>
-                <box size="${body_length} ${body_width} ${body_height}"/>
-            </geometry>
-        </collision>
-        <inertial>
-            <origin rpy="0 0 0" xyz="${body_com_x} ${body_com_y} ${body_com_z}"/>
-            <mass value="${body_mass}"/>
-            <inertia ixx="${body_ixx}" ixy="${body_ixy}" ixz="${body_ixz}" iyy="${body_iyy}" iyz="${body_iyz}"
-                     izz="${body_izz}"/>
-        </inertial>
-    </link>
-
-    <joint name="imu_joint" type="fixed">
-        <parent link="body"/>
-        <child link="imu_link"/>
-        <origin rpy="0 0 0" xyz="33.4e-3 -17.2765e-3 51.0469e-3"/>
-    </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>
-        </visual>
-        <collision>
-            <origin rpy="0 0 0" xyz="0 0 0"/>
-            <geometry>
-                <box size=".001 .001 .001"/>
-            </geometry>
-        </collision>
-    </link>
-
-    <joint name="scan_joint" type="fixed">
-        <parent link="body"/>
-        <child link="lidar_link"/>
-        <origin rpy="0 0 0" xyz="0.21425 0 0.0908"/>
-    </joint>
-
-    <link name="lidar_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>
-    </link>
-
-    <joint name="D435_camera_joint" type="fixed">
-        <parent link="body"/>
-        <child link="D435_camera_link"/>
-        <origin rpy="0 0 0" xyz="271.994e-3 25e-3 114.912e-3"/>
-    </joint>
-
-    <link name="D435_camera_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>
-        </visual>
-        <collision>
-            <origin rpy="0 0 0" xyz="0 0 0"/>
-            <geometry>
-                <box size=".001 .001 .001"/>
-            </geometry>
-        </collision>
-    </link>
-
-    <joint name="RGB_camera_joint" type="fixed">
-        <parent link="body"/>
-        <child link="RGB_camera_link"/>
-        <origin rpy="0 0 0" xyz="275.76e-3 0 125.794e-3"/>
-    </joint>
-
-    <link name="RGB_camera_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>
-        </visual>
-        <collision>
-            <origin rpy="0 0 0" xyz="0 0 0"/>
-            <geometry>
-                <box size=".001 .001 .001"/>
-            </geometry>
-        </collision>
-    </link>
-
-    <joint name="AI_camera_joint" type="fixed">
-        <parent link="body"/>
-        <child link="AI_camera_link"/>
-        <origin rpy="0 0 0" xyz="290.228e-3 0 147.420e-3"/>
-    </joint>
-
-    <link name="AI_camera_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>
-        </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">
-        <origin rpy="0 0 0" xyz="${abad_offset_x} ${-abad_offset_y} 0"/>
-    </xacro:leg>
-
-    <xacro:leg name="FL" mirror="1" mirror_dae="True" front_hind="1" front_hind_dae="True">
-        <origin rpy="0 0 0" xyz="${abad_offset_x} ${abad_offset_y} 0"/>
-    </xacro:leg>
-
-    <xacro:leg name="RR" mirror="-1" mirror_dae="False" front_hind="-1" front_hind_dae="False">
-        <origin rpy="0 0 0" xyz="${-abad_offset_x} ${-abad_offset_y} 0"/>
-    </xacro:leg>
-
-    <xacro:leg name="RL" mirror="1" mirror_dae="True" front_hind="-1" front_hind_dae="False">
-        <origin rpy="0 0 0" xyz="${-abad_offset_x} ${abad_offset_y} 0"/>
-    </xacro:leg>
-
-    <!-- This link is only for head collision -->
-    <joint name="head_joint" type="fixed">
-        <parent link="body"/>
-        <child link="head"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-    </joint>
-    <link name="head">
-        <collision>
-            <origin rpy="0 0 0" xyz="0.256 0 0.120"/>
-            <geometry>
-                <box size="0.076 0.060 0.040"/>
-            </geometry>
-        </collision>
-        <collision>
-            <origin rpy="0 0 0" xyz="0.225 0 0.150"/>
-            <geometry>
-                <box size="0.020 0.080 0.100"/>
-            </geometry>
-        </collision>
-    </link>
-
-    <gazebo reference="body">
-        <visual>
-            <material>
-                <ambient>.1 .1 .1 1.0</ambient>
-                <diffuse>.1 .1 .1 1.0</diffuse>
-                <specular>.1 .1 .1 1.0</specular>
-            </material>
-        </visual>
-    </gazebo>
-
-</robot>
\ No newline at end of file
diff --git a/descriptions/cyberdog_description/xacro/ros2_control.xacro b/descriptions/cyberdog_description/xacro/ros2_control.xacro
deleted file mode 100644
index 78f4c47..0000000
--- a/descriptions/cyberdog_description/xacro/ros2_control.xacro
+++ /dev/null
@@ -1,149 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
-
-    <!-- <xacro:macro name="leg" params="name mirror mirror_dae front_hind front_hind_dae"> -->
-    <ros2_control name="GazeboSystem" type="system">
-        <hardware>
-            <plugin>gz_ros2_control/GazeboSimSystem</plugin>
-        </hardware>
-
-        <joint name="FR_abad_joint">
-            <command_interface name="effort" initial_value="0.0"/>
-            <state_interface name="position"/>
-            <state_interface name="velocity"/>
-            <state_interface name="effort"/>
-        </joint>
-
-        <joint name="FR_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="FR_knee_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_abad_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_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_knee_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_abad_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_knee_joint">
-            <command_interface name="effort" initial_value="0.0"/>
-            <state_interface name="position"/>
-            <state_interface name="velocity"/>
-            <state_interface name="effort"/>
-        </joint>
-
-        <joint name="RL_abad_joint">
-            <command_interface name="effort" initial_value="0.0"/>
-            <state_interface name="position"/>
-            <state_interface name="velocity"/>
-            <state_interface name="effort"/>
-        </joint>
-
-        <joint name="RL_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="RL_knee_joint">
-            <command_interface name="effort" initial_value="0.0"/>
-            <state_interface name="position"/>
-            <state_interface name="velocity"/>
-            <state_interface name="effort"/>
-        </joint>
-
-        <sensor name="imu_sensor">
-            <state_interface name="orientation.x"/>
-            <state_interface name="orientation.y"/>
-            <state_interface name="orientation.z"/>
-            <state_interface name="orientation.w"/>
-            <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>
-
-        <sensor name="LF_foot_end">
-            <state_interface name="force.x"/>
-            <state_interface name="force.y"/>
-            <state_interface name="force.z"/>
-            <param name="frame_id">LF_FOOT</param>
-        </sensor>
-        <sensor name="RF_foot_end">
-            <state_interface name="force.x"/>
-            <state_interface name="force.y"/>
-            <state_interface name="force.z"/>
-            <param name="frame_id">RF_FOOT</param>
-        </sensor>
-        <sensor name="LH_foot_end">
-            <state_interface name="force.x"/>
-            <state_interface name="force.y"/>
-            <state_interface name="force.z"/>
-            <param name="frame_id">LH_FOOT</param>
-        </sensor>
-        <sensor name="RH_foot_end">
-            <state_interface name="force.x"/>
-            <state_interface name="force.y"/>
-            <state_interface name="force.z"/>
-            <param name="frame_id">LH_FOOT</param>
-        </sensor>
-    </ros2_control>
-
-    <!-- Gazebo's ros2_control plugin  -->
-    <gazebo>
-        <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
-            <parameters>$(find cyberdog_description)/config/robot_control.yaml</parameters>
-        </plugin>
-        <plugin filename="gz-sim-imu-system"
-                name="gz::sim::systems::Imu">
-        </plugin>
-        <plugin filename="gz-sim-contact-system"
-                name="gz::sim::systems::Contact">
-        </plugin>
-        <plugin filename="gz-sim-forcetorque-system"
-                name="gz::sim::systems::ForceTorque">
-        </plugin>
-    </gazebo>
-
-</robot>
\ No newline at end of file
diff --git a/descriptions/cyberdog_description/xacro/transmission.xacro b/descriptions/cyberdog_description/xacro/transmission.xacro
deleted file mode 100644
index 18f60be..0000000
--- a/descriptions/cyberdog_description/xacro/transmission.xacro
+++ /dev/null
@@ -1,41 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
-
-    <xacro:macro name="leg_transmission" params="name">
-
-        <transmission name="${name}_abad_tran">
-            <type>transmission_interface/SimpleTransmission</type>
-            <joint name="${name}_abad_joint">
-            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-            </joint>
-            <actuator name="${name}_abad_motor">
-            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-            <mechanicalReduction>"${abadGearRatio}"</mechanicalReduction>
-            </actuator>
-        </transmission>
-
-        <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>"${abadGearRatio}"</mechanicalReduction>
-            </actuator>
-        </transmission>
-
-        <transmission name="${name}_knee_tran">
-            <type>transmission_interface/SimpleTransmission</type>
-            <joint name="${name}_knee_joint">
-            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-            </joint>
-            <actuator name="${name}_knee_motor">
-            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-            <mechanicalReduction>"${kneeGearRatio}"</mechanicalReduction>
-            </actuator>
-        </transmission>
-
-    </xacro:macro>
-
-</robot>
diff --git a/descriptions/aliengo_description/CMakeLists.txt b/descriptions/unitree/aliengo_description/CMakeLists.txt
similarity index 100%
rename from descriptions/aliengo_description/CMakeLists.txt
rename to descriptions/unitree/aliengo_description/CMakeLists.txt
diff --git a/descriptions/aliengo_description/README.md b/descriptions/unitree/aliengo_description/README.md
similarity index 94%
rename from descriptions/aliengo_description/README.md
rename to descriptions/unitree/aliengo_description/README.md
index f76b874..3fc4140 100644
--- a/descriptions/aliengo_description/README.md
+++ b/descriptions/unitree/aliengo_description/README.md
@@ -1,7 +1,7 @@
 # Unitree AlienGo Description
 This repository contains the urdf model of Aliengo.
 
-![Aliengo](../../.images/aliengo.png)
+![Aliengo](../../../.images/aliengo.png)
 
 Tested environment:
 * Ubuntu 24.04
diff --git a/descriptions/aliengo_description/config/ocs2/gait.info b/descriptions/unitree/aliengo_description/config/ocs2/gait.info
similarity index 100%
rename from descriptions/aliengo_description/config/ocs2/gait.info
rename to descriptions/unitree/aliengo_description/config/ocs2/gait.info
diff --git a/descriptions/aliengo_description/config/ocs2/reference.info b/descriptions/unitree/aliengo_description/config/ocs2/reference.info
similarity index 100%
rename from descriptions/aliengo_description/config/ocs2/reference.info
rename to descriptions/unitree/aliengo_description/config/ocs2/reference.info
diff --git a/descriptions/aliengo_description/config/ocs2/task.info b/descriptions/unitree/aliengo_description/config/ocs2/task.info
similarity index 99%
rename from descriptions/aliengo_description/config/ocs2/task.info
rename to descriptions/unitree/aliengo_description/config/ocs2/task.info
index 4a67c83..22ba39d 100644
--- a/descriptions/aliengo_description/config/ocs2/task.info
+++ b/descriptions/unitree/aliengo_description/config/ocs2/task.info
@@ -283,9 +283,9 @@ selfCollision
 ; Whole body control
 torqueLimitsTask
 {
-   (0,0)  35.278     ; HAA
-   (1,0)  35.278     ; HFE
-   (2,0)  44.4       ; KFE
+   (0,0)  12.0     ; HAA
+   (1,0)  12.0     ; HFE
+   (2,0)  12.0       ; KFE
 }
 
 frictionConeTask
diff --git a/descriptions/aliengo_description/config/robot_control.yaml b/descriptions/unitree/aliengo_description/config/robot_control.yaml
similarity index 100%
rename from descriptions/aliengo_description/config/robot_control.yaml
rename to descriptions/unitree/aliengo_description/config/robot_control.yaml
diff --git a/descriptions/aliengo_description/config/visualize_urdf.rviz b/descriptions/unitree/aliengo_description/config/visualize_urdf.rviz
similarity index 100%
rename from descriptions/aliengo_description/config/visualize_urdf.rviz
rename to descriptions/unitree/aliengo_description/config/visualize_urdf.rviz
diff --git a/descriptions/aliengo_description/launch/ocs2_control.launch.py b/descriptions/unitree/aliengo_description/launch/ocs2_control.launch.py
similarity index 100%
rename from descriptions/aliengo_description/launch/ocs2_control.launch.py
rename to descriptions/unitree/aliengo_description/launch/ocs2_control.launch.py
diff --git a/descriptions/aliengo_description/launch/unitree_guide.launch.py b/descriptions/unitree/aliengo_description/launch/unitree_guide.launch.py
similarity index 100%
rename from descriptions/aliengo_description/launch/unitree_guide.launch.py
rename to descriptions/unitree/aliengo_description/launch/unitree_guide.launch.py
diff --git a/descriptions/aliengo_description/launch/visualize.launch.py b/descriptions/unitree/aliengo_description/launch/visualize.launch.py
similarity index 100%
rename from descriptions/aliengo_description/launch/visualize.launch.py
rename to descriptions/unitree/aliengo_description/launch/visualize.launch.py
diff --git a/descriptions/aliengo_description/meshes/calf.dae b/descriptions/unitree/aliengo_description/meshes/calf.dae
similarity index 100%
rename from descriptions/aliengo_description/meshes/calf.dae
rename to descriptions/unitree/aliengo_description/meshes/calf.dae
diff --git a/descriptions/aliengo_description/meshes/hip.dae b/descriptions/unitree/aliengo_description/meshes/hip.dae
similarity index 100%
rename from descriptions/aliengo_description/meshes/hip.dae
rename to descriptions/unitree/aliengo_description/meshes/hip.dae
diff --git a/descriptions/aliengo_description/meshes/thigh.dae b/descriptions/unitree/aliengo_description/meshes/thigh.dae
similarity index 100%
rename from descriptions/aliengo_description/meshes/thigh.dae
rename to descriptions/unitree/aliengo_description/meshes/thigh.dae
diff --git a/descriptions/aliengo_description/meshes/thigh_mirror.dae b/descriptions/unitree/aliengo_description/meshes/thigh_mirror.dae
similarity index 100%
rename from descriptions/aliengo_description/meshes/thigh_mirror.dae
rename to descriptions/unitree/aliengo_description/meshes/thigh_mirror.dae
diff --git a/descriptions/aliengo_description/meshes/trunk.dae b/descriptions/unitree/aliengo_description/meshes/trunk.dae
similarity index 100%
rename from descriptions/aliengo_description/meshes/trunk.dae
rename to descriptions/unitree/aliengo_description/meshes/trunk.dae
diff --git a/descriptions/aliengo_description/meshes/trunk_uv_base_final.png b/descriptions/unitree/aliengo_description/meshes/trunk_uv_base_final.png
similarity index 100%
rename from descriptions/aliengo_description/meshes/trunk_uv_base_final.png
rename to descriptions/unitree/aliengo_description/meshes/trunk_uv_base_final.png
diff --git a/descriptions/aliengo_description/package.xml b/descriptions/unitree/aliengo_description/package.xml
similarity index 100%
rename from descriptions/aliengo_description/package.xml
rename to descriptions/unitree/aliengo_description/package.xml
diff --git a/descriptions/aliengo_description/urdf/robot.urdf b/descriptions/unitree/aliengo_description/urdf/robot.urdf
similarity index 100%
rename from descriptions/aliengo_description/urdf/robot.urdf
rename to descriptions/unitree/aliengo_description/urdf/robot.urdf
diff --git a/descriptions/aliengo_description/xacro/const.xacro b/descriptions/unitree/aliengo_description/xacro/const.xacro
similarity index 100%
rename from descriptions/aliengo_description/xacro/const.xacro
rename to descriptions/unitree/aliengo_description/xacro/const.xacro
diff --git a/descriptions/aliengo_description/xacro/gazebo.xacro b/descriptions/unitree/aliengo_description/xacro/gazebo.xacro
similarity index 100%
rename from descriptions/aliengo_description/xacro/gazebo.xacro
rename to descriptions/unitree/aliengo_description/xacro/gazebo.xacro
diff --git a/descriptions/aliengo_description/xacro/leg.xacro b/descriptions/unitree/aliengo_description/xacro/leg.xacro
similarity index 100%
rename from descriptions/aliengo_description/xacro/leg.xacro
rename to descriptions/unitree/aliengo_description/xacro/leg.xacro
diff --git a/descriptions/aliengo_description/xacro/materials.xacro b/descriptions/unitree/aliengo_description/xacro/materials.xacro
similarity index 100%
rename from descriptions/aliengo_description/xacro/materials.xacro
rename to descriptions/unitree/aliengo_description/xacro/materials.xacro
diff --git a/descriptions/aliengo_description/xacro/robot.xacro b/descriptions/unitree/aliengo_description/xacro/robot.xacro
similarity index 100%
rename from descriptions/aliengo_description/xacro/robot.xacro
rename to descriptions/unitree/aliengo_description/xacro/robot.xacro
diff --git a/descriptions/aliengo_description/xacro/ros2_control.xacro b/descriptions/unitree/aliengo_description/xacro/ros2_control.xacro
similarity index 100%
rename from descriptions/aliengo_description/xacro/ros2_control.xacro
rename to descriptions/unitree/aliengo_description/xacro/ros2_control.xacro
diff --git a/descriptions/aliengo_description/xacro/stairs.xacro b/descriptions/unitree/aliengo_description/xacro/stairs.xacro
similarity index 100%
rename from descriptions/aliengo_description/xacro/stairs.xacro
rename to descriptions/unitree/aliengo_description/xacro/stairs.xacro
diff --git a/descriptions/aliengo_description/xacro/transmission.xacro b/descriptions/unitree/aliengo_description/xacro/transmission.xacro
similarity index 100%
rename from descriptions/aliengo_description/xacro/transmission.xacro
rename to descriptions/unitree/aliengo_description/xacro/transmission.xacro
diff --git a/descriptions/b2_description/CMakeLists.txt b/descriptions/unitree/b2_description/CMakeLists.txt
similarity index 94%
rename from descriptions/b2_description/CMakeLists.txt
rename to descriptions/unitree/b2_description/CMakeLists.txt
index 0df7242..82a74d9 100644
--- a/descriptions/b2_description/CMakeLists.txt
+++ b/descriptions/unitree/b2_description/CMakeLists.txt
@@ -1,12 +1,12 @@
-cmake_minimum_required(VERSION 3.8)
-
-project(b2_description)
-
-find_package(ament_cmake REQUIRED)
-
-install(
-		DIRECTORY meshes xacro launch config urdf
-		DESTINATION share/${PROJECT_NAME}/
-)
-
-ament_package()
+cmake_minimum_required(VERSION 3.8)
+
+project(b2_description)
+
+find_package(ament_cmake REQUIRED)
+
+install(
+		DIRECTORY meshes xacro launch config urdf
+		DESTINATION share/${PROJECT_NAME}/
+)
+
+ament_package()
diff --git a/descriptions/b2_description/README.md b/descriptions/unitree/b2_description/README.md
similarity index 95%
rename from descriptions/b2_description/README.md
rename to descriptions/unitree/b2_description/README.md
index f2ff9fd..eb7c900 100644
--- a/descriptions/b2_description/README.md
+++ b/descriptions/unitree/b2_description/README.md
@@ -1,7 +1,7 @@
 # Unitree B2 Description
 This repository contains the urdf model of b2.
 
-![B2](../../.images/b2.png)
+![B2](../../../.images/b2.png)
 
 ## Build
 ```bash
diff --git a/descriptions/b2_description/config/ocs2/gait.info b/descriptions/unitree/b2_description/config/ocs2/gait.info
similarity index 100%
rename from descriptions/b2_description/config/ocs2/gait.info
rename to descriptions/unitree/b2_description/config/ocs2/gait.info
diff --git a/descriptions/b2_description/config/ocs2/reference.info b/descriptions/unitree/b2_description/config/ocs2/reference.info
similarity index 100%
rename from descriptions/b2_description/config/ocs2/reference.info
rename to descriptions/unitree/b2_description/config/ocs2/reference.info
diff --git a/descriptions/b2_description/config/ocs2/task.info b/descriptions/unitree/b2_description/config/ocs2/task.info
similarity index 100%
rename from descriptions/b2_description/config/ocs2/task.info
rename to descriptions/unitree/b2_description/config/ocs2/task.info
diff --git a/descriptions/b2_description/config/robot_control.yaml b/descriptions/unitree/b2_description/config/robot_control.yaml
similarity index 100%
rename from descriptions/b2_description/config/robot_control.yaml
rename to descriptions/unitree/b2_description/config/robot_control.yaml
diff --git a/descriptions/b2_description/config/visualize_urdf.rviz b/descriptions/unitree/b2_description/config/visualize_urdf.rviz
similarity index 100%
rename from descriptions/b2_description/config/visualize_urdf.rviz
rename to descriptions/unitree/b2_description/config/visualize_urdf.rviz
diff --git a/descriptions/b2_description/launch/ocs2_control.launch.py b/descriptions/unitree/b2_description/launch/ocs2_control.launch.py
similarity index 100%
rename from descriptions/b2_description/launch/ocs2_control.launch.py
rename to descriptions/unitree/b2_description/launch/ocs2_control.launch.py
diff --git a/descriptions/b2_description/launch/unitree_guide.launch.py b/descriptions/unitree/b2_description/launch/unitree_guide.launch.py
similarity index 100%
rename from descriptions/b2_description/launch/unitree_guide.launch.py
rename to descriptions/unitree/b2_description/launch/unitree_guide.launch.py
diff --git a/descriptions/b2_description/launch/visualize.launch.py b/descriptions/unitree/b2_description/launch/visualize.launch.py
similarity index 100%
rename from descriptions/b2_description/launch/visualize.launch.py
rename to descriptions/unitree/b2_description/launch/visualize.launch.py
diff --git a/descriptions/b2_description/meshes/FL_calf.dae b/descriptions/unitree/b2_description/meshes/FL_calf.dae
similarity index 100%
rename from descriptions/b2_description/meshes/FL_calf.dae
rename to descriptions/unitree/b2_description/meshes/FL_calf.dae
diff --git a/descriptions/b2_description/meshes/FL_foot.dae b/descriptions/unitree/b2_description/meshes/FL_foot.dae
similarity index 100%
rename from descriptions/b2_description/meshes/FL_foot.dae
rename to descriptions/unitree/b2_description/meshes/FL_foot.dae
diff --git a/descriptions/b2_description/meshes/FL_hip.dae b/descriptions/unitree/b2_description/meshes/FL_hip.dae
similarity index 100%
rename from descriptions/b2_description/meshes/FL_hip.dae
rename to descriptions/unitree/b2_description/meshes/FL_hip.dae
diff --git a/descriptions/b2_description/meshes/FL_thigh.dae b/descriptions/unitree/b2_description/meshes/FL_thigh.dae
similarity index 100%
rename from descriptions/b2_description/meshes/FL_thigh.dae
rename to descriptions/unitree/b2_description/meshes/FL_thigh.dae
diff --git a/descriptions/b2_description/meshes/FR_calf.dae b/descriptions/unitree/b2_description/meshes/FR_calf.dae
similarity index 100%
rename from descriptions/b2_description/meshes/FR_calf.dae
rename to descriptions/unitree/b2_description/meshes/FR_calf.dae
diff --git a/descriptions/b2_description/meshes/FR_hip.dae b/descriptions/unitree/b2_description/meshes/FR_hip.dae
similarity index 100%
rename from descriptions/b2_description/meshes/FR_hip.dae
rename to descriptions/unitree/b2_description/meshes/FR_hip.dae
diff --git a/descriptions/b2_description/meshes/FR_thigh.dae b/descriptions/unitree/b2_description/meshes/FR_thigh.dae
similarity index 100%
rename from descriptions/b2_description/meshes/FR_thigh.dae
rename to descriptions/unitree/b2_description/meshes/FR_thigh.dae
diff --git a/descriptions/b2_description/meshes/RL_calf.dae b/descriptions/unitree/b2_description/meshes/RL_calf.dae
similarity index 100%
rename from descriptions/b2_description/meshes/RL_calf.dae
rename to descriptions/unitree/b2_description/meshes/RL_calf.dae
diff --git a/descriptions/b2_description/meshes/RL_hip.dae b/descriptions/unitree/b2_description/meshes/RL_hip.dae
similarity index 100%
rename from descriptions/b2_description/meshes/RL_hip.dae
rename to descriptions/unitree/b2_description/meshes/RL_hip.dae
diff --git a/descriptions/b2_description/meshes/RL_thigh.dae b/descriptions/unitree/b2_description/meshes/RL_thigh.dae
similarity index 100%
rename from descriptions/b2_description/meshes/RL_thigh.dae
rename to descriptions/unitree/b2_description/meshes/RL_thigh.dae
diff --git a/descriptions/b2_description/meshes/RR_calf.dae b/descriptions/unitree/b2_description/meshes/RR_calf.dae
similarity index 100%
rename from descriptions/b2_description/meshes/RR_calf.dae
rename to descriptions/unitree/b2_description/meshes/RR_calf.dae
diff --git a/descriptions/b2_description/meshes/RR_hip.dae b/descriptions/unitree/b2_description/meshes/RR_hip.dae
similarity index 100%
rename from descriptions/b2_description/meshes/RR_hip.dae
rename to descriptions/unitree/b2_description/meshes/RR_hip.dae
diff --git a/descriptions/b2_description/meshes/RR_thigh.dae b/descriptions/unitree/b2_description/meshes/RR_thigh.dae
similarity index 100%
rename from descriptions/b2_description/meshes/RR_thigh.dae
rename to descriptions/unitree/b2_description/meshes/RR_thigh.dae
diff --git a/descriptions/b2_description/meshes/base_link.dae b/descriptions/unitree/b2_description/meshes/base_link.dae
similarity index 100%
rename from descriptions/b2_description/meshes/base_link.dae
rename to descriptions/unitree/b2_description/meshes/base_link.dae
diff --git a/descriptions/b2_description/meshes/calf.dae b/descriptions/unitree/b2_description/meshes/calf.dae
similarity index 100%
rename from descriptions/b2_description/meshes/calf.dae
rename to descriptions/unitree/b2_description/meshes/calf.dae
diff --git a/descriptions/b2_description/meshes/hip.dae b/descriptions/unitree/b2_description/meshes/hip.dae
similarity index 100%
rename from descriptions/b2_description/meshes/hip.dae
rename to descriptions/unitree/b2_description/meshes/hip.dae
diff --git a/descriptions/b2_description/meshes/thigh.dae b/descriptions/unitree/b2_description/meshes/thigh.dae
similarity index 100%
rename from descriptions/b2_description/meshes/thigh.dae
rename to descriptions/unitree/b2_description/meshes/thigh.dae
diff --git a/descriptions/b2_description/meshes/thigh_mirror.dae b/descriptions/unitree/b2_description/meshes/thigh_mirror.dae
similarity index 100%
rename from descriptions/b2_description/meshes/thigh_mirror.dae
rename to descriptions/unitree/b2_description/meshes/thigh_mirror.dae
diff --git a/descriptions/b2_description/meshes/trunk.dae b/descriptions/unitree/b2_description/meshes/trunk.dae
similarity index 100%
rename from descriptions/b2_description/meshes/trunk.dae
rename to descriptions/unitree/b2_description/meshes/trunk.dae
diff --git a/descriptions/b2_description/meshes/trunk1.dae b/descriptions/unitree/b2_description/meshes/trunk1.dae
similarity index 100%
rename from descriptions/b2_description/meshes/trunk1.dae
rename to descriptions/unitree/b2_description/meshes/trunk1.dae
diff --git a/descriptions/b2_description/meshes/trunk2.dae b/descriptions/unitree/b2_description/meshes/trunk2.dae
similarity index 100%
rename from descriptions/b2_description/meshes/trunk2.dae
rename to descriptions/unitree/b2_description/meshes/trunk2.dae
diff --git a/descriptions/b2_description/package.xml b/descriptions/unitree/b2_description/package.xml
similarity index 95%
rename from descriptions/b2_description/package.xml
rename to descriptions/unitree/b2_description/package.xml
index 5b24d38..ed64635 100644
--- a/descriptions/b2_description/package.xml
+++ b/descriptions/unitree/b2_description/package.xml
@@ -1,20 +1,20 @@
-<package format="2">
-  <name>b2_description</name>
-  <version>1.0.0</version>
-  <description>
-    <p>URDF Description package for b2_description</p>
-    <p>This package contains configuration data, 3D models and launch files
-for b2_description robot</p>
-  </description>
-  <author>Unitree Robotics</author>
-  <maintainer email="TODO@email.com" />
-  <license>BSD</license>
-
-  <exec_depend>xacro</exec_depend>
-  <exec_depend>joint_state_publisher</exec_depend>
-  <exec_depend>robot_state_publisher</exec_depend>
-  <exec_depend>imu_sensor_broadcaster</exec_depend>
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
+<package format="2">
+  <name>b2_description</name>
+  <version>1.0.0</version>
+  <description>
+    <p>URDF Description package for b2_description</p>
+    <p>This package contains configuration data, 3D models and launch files
+for b2_description robot</p>
+  </description>
+  <author>Unitree Robotics</author>
+  <maintainer email="TODO@email.com" />
+  <license>BSD</license>
+
+  <exec_depend>xacro</exec_depend>
+  <exec_depend>joint_state_publisher</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>imu_sensor_broadcaster</exec_depend>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
 </package>
\ No newline at end of file
diff --git a/descriptions/b2_description/urdf/robot.urdf b/descriptions/unitree/b2_description/urdf/robot.urdf
similarity index 95%
rename from descriptions/b2_description/urdf/robot.urdf
rename to descriptions/unitree/b2_description/urdf/robot.urdf
index 70bd841..0e6759e 100644
--- a/descriptions/b2_description/urdf/robot.urdf
+++ b/descriptions/unitree/b2_description/urdf/robot.urdf
@@ -1,1533 +1,1533 @@
-<?xml version="1.0" encoding="utf-8"?>
-<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) 
-     Commit Version: 1.6.0-4-g7f85cfe  Build Version: 1.6.7995.38578
-     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
-<robot
-  name="b2_description">
-  <link
-    name="base_link">
-    <inertial>
-      <origin
-        xyz="0.000458 0.005261 0.000665"
-        rpy="0 0 0" />
-      <mass
-        value="35.86" />
-      <inertia
-        ixx="0.27466"
-        ixy="-0.000622"
-        ixz="0.00315"
-        iyy="1.0618"
-        iyz="-0.00139"
-        izz="1.1825" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://b2_description/meshes/base_link.dae" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-        <geometry>
-          <box size="0.5 0.28 0.15" />
-        </geometry>
-    </collision>
-  </link>
-  <link
-    name="lidar_link">
-    <inertial>
-      <origin
-        xyz="0.34218 0 0.17851"
-        rpy="0 0 0" />
-      <mass
-        value="4.19E-15" />
-      <inertia
-        ixx="1.68E-27"
-        ixy="-3.2E-45"
-        ixz="7.68E-44"
-        iyy="1.68E-27"
-        iyz="-9.26E-47"
-        izz="1.68E-27" />
-    </inertial>
-    <collision>
-      <origin
-        xyz="0 0 -0.02"
-        rpy="0 0 0" />
-      <geometry>
-        <cylinder length="0.16" radius="0.076" />
-        <!-- <mesh
-          filename="package://b2_description/meshes/lidar_link.STL" /> -->
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint_lidar"
-    type="fixed">
-    <origin
-      xyz="0.34218 0 0.17851"
-      rpy="0 0 0" />
-    <parent
-      link="base_link" />
-    <child
-      link="lidar_link" />
-    <axis
-      xyz="0 0 0" />
-  </joint>
-  <link
-    name="f_dc_link">
-    <inertial>
-      <origin
-        xyz="3.47E-18 0 5.55E-17"
-        rpy="0 0 0" />
-      <mass
-        value="4.19E-15" />
-      <inertia
-        ixx="1.68E-27"
-        ixy="-9.81E-45"
-        ixz="-2.54E-44"
-        iyy="1.68E-27"
-        iyz="0"
-        izz="1.68E-27" />
-    </inertial>
-  </link>
-  <joint
-    name="joint_f_dc"
-    type="fixed">
-    <origin
-      xyz="0.41251 0.024997 0.04765"
-      rpy="-2.3562 0 -1.5708" />
-    <parent
-      link="base_link" />
-    <child
-      link="f_dc_link" />
-    <axis
-      xyz="0 0 0" />
-  </joint>
-  <link
-    name="r_dc_link">
-    <inertial>
-      <origin
-        xyz="-7.46E-16 -3.33E-16 -1.11E-15"
-        rpy="0 0 0" />
-      <mass
-        value="4.19E-15" />
-      <inertia
-        ixx="1.68E-27"
-        ixy="-4.31E-44"
-        ixz="2.48E-45"
-        iyy="1.68E-27"
-        iyz="0"
-        izz="1.68E-27" />
-    </inertial>
-  </link>
-  <joint
-    name="joint_r_dc"
-    type="fixed">
-    <origin
-      xyz="-0.4081 -0.025 0.024861"
-      rpy="-2.3557 0 1.5708" />
-    <parent
-      link="base_link" />
-    <child
-      link="r_dc_link" />
-    <axis
-      xyz="0 0 0" />
-  </joint>
-  <link
-    name="imu_link">
-    <inertial>
-      <origin
-        xyz="-2.02E-17 1.73E-17 -1.39E-17"
-        rpy="0 0 0" />
-      <mass
-        value="4.19E-15" />
-      <inertia
-        ixx="1.68E-27"
-        ixy="1.84E-44"
-        ixz="-7.35E-44"
-        iyy="1.68E-27"
-        iyz="-3.67E-44"
-        izz="1.68E-27" />
-    </inertial>
-  </link>
-  <joint
-    name="joint_imu"
-    type="fixed">
-    <origin
-      xyz="0 -0.02341 0.04927"
-      rpy="0 0 0" />
-    <parent
-      link="base_link" />
-    <child
-      link="imu_link" />
-    <axis
-      xyz="0 0 0" />
-  </joint>
-  <link
-    name="head_Link">
-    <inertial>
-      <origin
-        xyz="0.33989 -0.000168 0.12029"
-        rpy="0 0 0" />
-      <mass
-        value="3.6885" />
-      <inertia
-        ixx="0.026455"
-        ixy="3.15E-05"
-        ixz="0.00166"
-        iyy="0.029221"
-        iyz="-3.27E-05"
-        izz="0.010918" />
-    </inertial>
-    <collision>
-      <origin
-        xyz="0.41 0 0.005"
-        rpy="0 0 0" />
-      <geometry>
-        <box size="0.04 0.12 0.14" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint_head"
-    type="fixed">
-    <origin
-      xyz="0 0 0"
-      rpy="0 0 0" />
-    <parent
-      link="base_link" />
-    <child
-      link="head_Link" />
-    <axis
-      xyz="0 0 0" />
-  </joint>
-  <link
-    name="tail_link">
-    <inertial>
-      <origin
-        xyz="-0.37448 0.000488 0.006495"
-        rpy="0 0 0" />
-      <mass
-        value="0.795" />
-      <inertia
-        ixx="0.0028118"
-        ixy="1.95E-05"
-        ixz="-7.87E-06"
-        iyy="0.0047506"
-        iyz="-1.57E-05"
-        izz="0.0029154" />
-    </inertial>
-    <collision>
-      <origin
-        xyz="-0.405 0 0.005"
-        rpy="0 0 0" />
-      <geometry>
-        <box size="0.025 0.12 0.14" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint_tail"
-    type="fixed">
-    <origin
-      xyz="0 0 0"
-      rpy="0 0 0" />
-    <parent
-      link="base_link" />
-    <child
-      link="tail_link" />
-    <axis
-      xyz="0 0 0" />
-  </joint>
-  <link
-    name="FL_hip">
-    <inertial>
-      <origin
-        xyz="-0.003841 -0.009068 0"
-        rpy="0 0 0" />
-      <mass
-        value="2.673" />
-      <inertia
-        ixx="0.0033188"
-        ixy="7.16E-05"
-        ixz="-3.77E-07"
-        iyy="0.0048743"
-        iyz="4E-09"
-        izz="0.0037087" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://b2_description/meshes/FL_hip.dae" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0.12 0"
-        rpy="1.5707963267948966 0 0" />
-      <geometry>
-        <cylinder length="0.05" radius="0.07" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="FL_hip_joint"
-    type="revolute">
-    <origin
-      xyz="0.3285 0.072 0"
-      rpy="0 0 0" />
-    <parent
-      link="base_link" />
-    <child
-      link="FL_hip" />
-    <axis
-      xyz="1 0 0" />
-    <limit
-      lower="-0.87"
-      upper="0.87"
-      effort="200"
-      velocity="23" />
-  </joint>
-  <link name="FL_hip_rotor">
-    <!-- <visual>
-      <origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.05"/>
-      </geometry>
-    </visual> -->
-    <collision>
-      <origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.05"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-      <mass value="0.2734"/>
-      <inertia ixx="0.000144463" ixy="0.0" ixz="0.0" iyy="0.000144463" iyz="0.0" izz="0.000263053"/>
-    </inertial>
-  </link>
-  <joint name="FL_hip_rotor_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0.20205 0.072 0"/>
-    <parent link="base_link"/>
-    <child link="FL_hip_rotor"/>
-  </joint>
-  <link
-    name="FL_thigh">
-    <inertial>
-      <origin
-        xyz="-0.006279 -0.032049 -0.057835"
-        rpy="0 0 0" />
-      <mass
-        value="4.536" />
-      <inertia
-        ixx="0.062299"
-        ixy="0.00087781"
-        ixz="-0.0036475"
-        iyy="0.061399"
-        iyz="0.0083537"
-        izz="0.0081997" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://b2_description/meshes/FL_thigh.dae" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="-0.025 0 -0.16"
-        rpy="0 1.57 0" />
-      <geometry>
-        <box size="0.32 0.0445 0.054" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="FL_thigh_joint"
-    type="revolute">
-    <origin
-      xyz="0 0.11973 0"
-      rpy="0 0 0" />
-    <parent
-      link="FL_hip" />
-    <child
-      link="FL_thigh" />
-    <axis
-      xyz="0 1 0" />
-    <limit
-      lower="-0.94"
-      upper="4.69"
-      effort="200"
-      velocity="23" />
-  </joint>
-  <link name="FL_thigh_rotor">
-    <!-- <visual>
-      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.05"/>
-      </geometry>
-    </visual> -->
-    <collision>
-      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.05"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-      <mass value="0.2734"/>
-      <inertia ixx="0.000144463" ixy="0.0" ixz="0.0" iyy="0.000144463" iyz="0.0" izz="0.000263053"/>
-    </inertial>
-  </link>
-  <joint name="FL_thigh_rotor_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 -0.00798 0"/>
-    <parent link="FL_hip"/>
-    <child link="FL_thigh_rotor"/>
-  </joint>
-  <link
-    name="FL_calf">
-    <inertial>
-      <origin
-        xyz="0.012422 0 -0.12499"
-        rpy="0 0 0" />
-      <mass
-        value="0.404" />
-      <inertia
-        ixx="0.01143"
-        ixy="0"
-        ixz="0.000643"
-        iyy="0.011534"
-        iyz="0"
-        izz="0.000331" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://b2_description/meshes/FL_calf.dae" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 1.2 0" xyz="-0.002 0 -0.03"/>
-      <geometry>
-        <box size="0.1 0.0245 0.018"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 1.35 0" xyz="0.022 0 -0.1"/>
-      <geometry>
-        <box size="0.1 0.0245 0.018"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 1.53 0" xyz="0.036 0 -0.225"/>
-      <geometry>
-        <box size="0.15 0.0245 0.015"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 2 0" xyz="0.023 0 -0.31"/>
-      <geometry>
-        <box size="0.05 0.028 0.035"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="FL_calf_joint"
-    type="revolute">
-    <origin
-      xyz="0 -8.6984E-05 -0.35"
-      rpy="0 0 0" />
-    <parent
-      link="FL_thigh" />
-    <child
-      link="FL_calf" />
-    <axis
-      xyz="0 1 0" />
-    <limit
-      lower="-2.82"
-      upper="-0.43"
-      effort="320"
-      velocity="14" />
-  </joint>
-  <link name="FL_calf_rotor">
-    <!-- <visual>
-      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.05"/>
-      </geometry>
-    </visual> -->
-    <collision>
-      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.05"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-      <mass value="0.2734"/>
-      <inertia ixx="0.000144463" ixy="0.0" ixz="0.0" iyy="0.000144463" iyz="0.0" izz="0.000263053"/>
-    </inertial>
-  </link>
-  <joint name="FL_calf_rotor_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 -0.05788 0"/>
-    <parent link="FL_thigh"/>
-    <child link="FL_calf_rotor"/>
-  </joint>
-  <link
-    name="FL_foot">
-    <inertial>
-      <origin
-        xyz="-0.006511 0 -0.010144"
-        rpy="0 0 0" />
-      <mass
-        value="0.126" />
-      <inertia
-        ixx="3.8E-05"
-        ixy="0"
-        ixz="1.1E-05"
-        iyy="4.1E-05"
-        iyz="0"
-        izz="4.2E-05" />
-    </inertial>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="1.57 0 0" />
-      <geometry>
-        <sphere radius="0.032" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="FL_foot_joint"
-    type="fixed">
-    <origin
-      xyz="0 0 -0.35"
-      rpy="0 0 0" />
-    <parent
-      link="FL_calf" />
-    <child
-      link="FL_foot" />
-  </joint>
-  <link
-    name="FR_hip">
-    <inertial>
-      <origin
-        xyz="-0.009305 0.010228 0.000264"
-        rpy="0 0 0" />
-      <mass
-        value="2.673" />
-      <inertia
-        ixx="0.0026431"
-        ixy="-0.00019234"
-        ixz="-6.76E-06"
-        iyy="0.0046728"
-        iyz="7.16E-06"
-        izz="0.0034208" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://b2_description/meshes/FR_hip.dae" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 -0.12 0"
-        rpy="1.5707963267948966 0 0" />
-      <geometry>
-        <cylinder length="0.05" radius="0.07" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="FR_hip_joint"
-    type="revolute">
-    <origin
-      xyz="0.3285 -0.072 0"
-      rpy="0 0 0" />
-    <parent
-      link="base_link" />
-    <child
-      link="FR_hip" />
-    <axis
-      xyz="1 0 0" />
-    <limit
-      lower="-0.87"
-      upper="0.87"
-      effort="200"
-      velocity="23" />
-  </joint>
-  <link name="FR_hip_rotor">
-    <!-- <visual>
-      <origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.05"/>
-      </geometry>
-    </visual> -->
-    <collision>
-      <origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.05"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-      <mass value="0.2734"/>
-      <inertia ixx="0.000144463" ixy="0.0" ixz="0.0" iyy="0.000144463" iyz="0.0" izz="0.000263053"/>
-    </inertial>
-  </link>
-  <joint name="FR_hip_rotor_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0.20205 -0.072 0"/>
-    <parent link="base_link"/>
-    <child link="FR_hip_rotor"/>
-  </joint>
-  <link
-    name="FR_thigh">
-    <inertial>
-      <origin
-        xyz="-0.006279 0.032049 -0.057835"
-        rpy="0 0 0" />
-      <mass
-        value="4.536" />
-      <inertia
-        ixx="0.06229908"
-        ixy="-0.000877808"
-        ixz="-0.003647464"
-        iyy="0.06139927"
-        iyz="-0.008353671"
-        izz="0.008199745" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://b2_description/meshes/FR_thigh.dae" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 1.57 0" xyz="-0.03 0 -0.2"/>
-      <geometry>
-        <box size="0.25 0.028 0.04"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="FR_thigh_joint"
-    type="revolute">
-    <origin
-      xyz="0 -0.11973 0"
-      rpy="0 0 0" />
-    <parent
-      link="FR_hip" />
-    <child
-      link="FR_thigh" />
-    <axis
-      xyz="0 1 0" />
-    <limit
-      lower="-0.94"
-      upper="4.69"
-      effort="200"
-      velocity="23" />
-  </joint>
-  <link name="FR_thigh_rotor">
-    <!-- <visual>
-      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.05"/>
-      </geometry>
-    </visual> -->
-    <collision>
-      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.05"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-      <mass value="0.2734"/>
-      <inertia ixx="0.000144463" ixy="0.0" ixz="0.0" iyy="0.000144463" iyz="0.0" izz="0.000263053"/>
-    </inertial>
-  </link>
-  <joint name="FR_thigh_rotor_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0.00798 0"/>
-    <parent link="FR_hip"/>
-    <child link="FR_thigh_rotor"/>
-  </joint>
-  <link
-    name="FR_calf">
-    <inertial>
-      <origin
-        xyz="0.012422 0 -0.12499"
-        rpy="0 0 0" />
-      <mass
-        value="0.404" />
-      <inertia
-        ixx="0.01143"
-        ixy="0"
-        ixz="0.000643"
-        iyy="0.011534"
-        iyz="0"
-        izz="0.000331" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://b2_description/meshes/FR_calf.dae" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 1.2 0" xyz="-0.002 0 -0.03"/>
-      <geometry>
-        <box size="0.1 0.0245 0.018"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 1.35 0" xyz="0.022 0 -0.1"/>
-      <geometry>
-        <box size="0.1 0.0245 0.018"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 1.53 0" xyz="0.036 0 -0.225"/>
-      <geometry>
-        <box size="0.15 0.0245 0.015"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 2 0" xyz="0.023 0 -0.31"/>
-      <geometry>
-        <box size="0.05 0.028 0.035"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="FR_calf_joint"
-    type="revolute">
-    <origin
-      xyz="0 8.6986E-05 -0.35"
-      rpy="0 0 0" />
-    <parent
-      link="FR_thigh" />
-    <child
-      link="FR_calf" />
-    <axis
-      xyz="0 1 0" />
-    <limit
-      lower="-2.82"
-      upper="-0.43"
-      effort="320"
-      velocity="14" />
-  </joint>
-  <link name="FR_calf_rotor">
-    <!-- <visual>
-      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.05"/>
-      </geometry>
-      <material name="green"/>
-    </visual> -->
-    <collision>
-      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.05"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-      <mass value="0.2734"/>
-      <inertia ixx="0.000144463" ixy="0.0" ixz="0.0" iyy="0.000144463" iyz="0.0" izz="0.000263053"/>
-    </inertial>
-  </link>
-  <joint name="FR_calf_rotor_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0.05788 0"/>
-    <parent link="FR_thigh"/>
-    <child link="FR_calf_rotor"/>
-  </joint>
-  <link
-    name="FR_foot">
-    <inertial>
-      <origin
-        xyz="-0.006511 0 -0.010144"
-        rpy="0 0 0" />
-      <mass
-        value="0.126" />
-      <inertia
-        ixx="3.8E-05"
-        ixy="0"
-        ixz="1.1E-05"
-        iyy="4.1E-05"
-        iyz="0"
-        izz="4.2E-05" />
-    </inertial>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="1.57 0 0" />
-      <geometry>
-        <sphere radius="0.032" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="FR_foot_joint"
-    type="fixed">
-    <origin
-      xyz="0 0 -0.35"
-      rpy="0 0 0" />
-    <parent
-      link="FR_calf" />
-    <child
-      link="FR_foot" />
-  </joint>
-  <link
-    name="RL_hip">
-    <inertial>
-      <origin
-        xyz="0.009305 -0.010228 0.000264"
-        rpy="0 0 0" />
-      <mass
-        value="2.673" />
-      <inertia
-        ixx="0.0026431"
-        ixy="-0.00019234"
-        ixz="6.76E-06"
-        iyy="0.0046728"
-        iyz="-7.16E-06"
-        izz="0.0034208" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://b2_description/meshes/RL_hip.dae" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0.12 0"
-        rpy="1.5707963267948966 0 0" />
-      <geometry>
-        <cylinder length="0.05" radius="0.07" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="RL_hip_joint"
-    type="revolute">
-    <origin
-      xyz="-0.3285 0.072 0"
-      rpy="0 0 0" />
-    <parent
-      link="base_link" />
-    <child
-      link="RL_hip" />
-    <axis
-      xyz="1 0 0" />
-    <limit
-      lower="-0.87"
-      upper="0.87"
-      effort="200"
-      velocity="23" />
-  </joint>
-  <link name="RL_hip_rotor">
-    <!-- <visual>
-      <origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.05"/>
-      </geometry>
-    </visual> -->
-    <collision>
-      <origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.05"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-      <mass value="0.2734"/>
-      <inertia ixx="0.000144463" ixy="0.0" ixz="0.0" iyy="0.000144463" iyz="0.0" izz="0.000263053"/>
-    </inertial>
-  </link>
-  <joint name="RL_hip_rotor_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="-0.20205 0.072 0"/>
-    <parent link="base_link"/>
-    <child link="RL_hip_rotor"/>
-  </joint>
-  <link
-    name="RL_thigh">
-    <inertial>
-      <origin
-        xyz="-0.006279 -0.032049 -0.057835"
-        rpy="0 0 0" />
-      <mass
-        value="4.536" />
-      <inertia
-        ixx="0.06229908"
-        ixy="0.000877808"
-        ixz="-0.003647464"
-        iyy="0.06139927"
-        iyz="0.008353671"
-        izz="0.008199745" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://b2_description/meshes/RL_thigh.dae" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 1.57 0" xyz="-0.03 0 -0.2"/>
-      <geometry>
-        <box size="0.25 0.028 0.04"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="RL_thigh_joint"
-    type="revolute">
-    <origin
-      xyz="0 0.11973 0"
-      rpy="0 0 0" />
-    <parent
-      link="RL_hip" />
-    <child
-      link="RL_thigh" />
-    <axis
-      xyz="0 1 0" />
-    <limit
-      lower="-0.94"
-      upper="4.69"
-      effort="200"
-      velocity="23" />
-  </joint>
-  <link name="RL_thigh_rotor">
-    <!-- <visual>
-      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.05"/>
-      </geometry>
-    </visual> -->
-    <collision>
-      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.05"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-      <mass value="0.2734"/>
-      <inertia ixx="0.000144463" ixy="0.0" ixz="0.0" iyy="0.000144463" iyz="0.0" izz="0.000263053"/>
-    </inertial>
-  </link>
-  <joint name="RL_thigh_rotor_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 -0.00798 0"/>
-    <parent link="RL_hip"/>
-    <child link="RL_thigh_rotor"/>
-  </joint>
-  <link
-    name="RL_calf">
-    <inertial>
-      <origin
-        xyz="0.012422 0 -0.12499"
-        rpy="0 0 0" />
-      <mass
-        value="0.404" />
-      <inertia
-        ixx="0.01143"
-        ixy="0"
-        ixz="0.000643"
-        iyy="0.011534"
-        iyz="0"
-        izz="0.000331" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://b2_description/meshes/RL_calf.dae" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 1.2 0" xyz="-0.002 0 -0.03"/>
-      <geometry>
-        <box size="0.1 0.0245 0.018"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 1.35 0" xyz="0.022 0 -0.1"/>
-      <geometry>
-        <box size="0.1 0.0245 0.018"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 1.53 0" xyz="0.036 0 -0.225"/>
-      <geometry>
-        <box size="0.15 0.0245 0.015"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 2 0" xyz="0.023 0 -0.31"/>
-      <geometry>
-        <box size="0.05 0.028 0.035"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="RL_calf_joint"
-    type="revolute">
-    <origin
-      xyz="0 -8.6984E-05 -0.35"
-      rpy="0 0 0" />
-    <parent
-      link="RL_thigh" />
-    <child
-      link="RL_calf" />
-    <axis
-      xyz="0 1 0" />
-    <limit
-      lower="-2.82"
-      upper="-0.43"
-      effort="320"
-      velocity="14" />
-  </joint>
-  <link name="RL_calf_rotor">
-    <!-- <visual>
-      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.05"/>
-      </geometry>
-    </visual> -->
-    <collision>
-      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.05"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-      <mass value="0.2734"/>
-      <inertia ixx="0.000144463" ixy="0.0" ixz="0.0" iyy="0.000144463" iyz="0.0" izz="0.000263053"/>
-    </inertial>
-  </link>
-  <joint name="RL_calf_rotor_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 -0.05788 0"/>
-    <parent link="RL_thigh"/>
-    <child link="RL_calf_rotor"/>
-  </joint>
-  <link
-    name="RL_foot">
-    <inertial>
-      <origin
-        xyz="-0.006511 0 -0.010144"
-        rpy="0 0 0" />
-      <mass
-        value="0.126" />
-      <inertia
-        ixx="3.8E-05"
-        ixy="0"
-        ixz="1.1E-05"
-        iyy="4.1E-05"
-        iyz="0"
-        izz="4.2E-05" />
-    </inertial>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="1.57 0 0" />
-      <geometry>
-        <sphere radius="0.032" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="RL_foot_joint"
-    type="fixed">
-    <origin
-      xyz="0 0 -0.35"
-      rpy="0 0 0" />
-    <parent
-      link="RL_calf" />
-    <child
-      link="RL_foot" />
-  </joint>
-  <link
-    name="RR_hip">
-    <inertial>
-      <origin
-        xyz="0.003841 0.009068 0"
-        rpy="0 0 0" />
-      <mass
-        value="2.673" />
-      <inertia
-        ixx="0.0033188"
-        ixy="7.16E-05"
-        ixz="3.77E-07"
-        iyy="0.0048743"
-        iyz="-4E-09"
-        izz="0.0037087" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://b2_description/meshes/RR_hip.dae" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 -0.12 0"
-        rpy="1.5707963267948966 0 0" />
-      <geometry>
-        <cylinder length="0.05" radius="0.07" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="RR_hip_joint"
-    type="revolute">
-    <origin
-      xyz="-0.3285 -0.072 0"
-      rpy="0 0 0" />
-    <parent
-      link="base_link" />
-    <child
-      link="RR_hip" />
-    <axis
-      xyz="1 0 0" />
-    <limit
-      lower="-0.87"
-      upper="0.87"
-      effort="200"
-      velocity="23" />
-  </joint>
-  <link name="RR_hip_rotor">
-    <!-- <visual>
-      <origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.05"/>
-      </geometry>
-    </visual> -->
-    <collision>
-      <origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.05"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-      <mass value="0.2734"/>
-      <inertia ixx="0.000144463" ixy="0.0" ixz="0.0" iyy="0.000144463" iyz="0.0" izz="0.000263053"/>
-    </inertial>
-  </link>
-  <joint name="RR_hip_rotor_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="-0.20205 -0.072 0"/>
-    <parent link="base_link"/>
-    <child link="RR_hip_rotor"/>
-  </joint>
-  <link
-    name="RR_thigh">
-    <inertial>
-      <origin
-        xyz="-0.006279 0.032049 -0.057835"
-        rpy="0 0 0" />
-      <mass
-        value="4.536" />
-      <inertia
-        ixx="0.062299"
-        ixy="-0.00087781"
-        ixz="-0.0036475"
-        iyy="0.061399"
-        iyz="-0.0083537"
-        izz="0.0081997" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://b2_description/meshes/RR_thigh.dae" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 1.57 0" xyz="-0.03 0 -0.2"/>
-      <geometry>
-        <box size="0.25 0.028 0.04"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="RR_thigh_joint"
-    type="revolute">
-    <origin
-      xyz="0 -0.11973 0"
-      rpy="0 0 0" />
-    <parent
-      link="RR_hip" />
-    <child
-      link="RR_thigh" />
-    <axis
-      xyz="0 1 0" />
-    <limit
-      lower="-0.94"
-      upper="4.69"
-      effort="200"
-      velocity="23" />
-  </joint>
-  <link name="RR_thigh_rotor">
-    <!-- <visual>
-      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.05"/>
-      </geometry>
-    </visual> -->
-    <collision>
-      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.05"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-      <mass value="0.2734"/>
-      <inertia ixx="0.000144463" ixy="0.0" ixz="0.0" iyy="0.000144463" iyz="0.0" izz="0.000263053"/>
-    </inertial>
-  </link>
-  <joint name="RR_thigh_rotor_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0.00798 0"/>
-    <parent link="RR_hip"/>
-    <child link="RR_thigh_rotor"/>
-  </joint>
-  <link
-    name="RR_calf">
-    <inertial>
-      <origin
-        xyz="0.012422 0 -0.12499"
-        rpy="0 0 0" />
-      <mass
-        value="0.404" />
-      <inertia
-        ixx="0.01143"
-        ixy="0"
-        ixz="0.000643"
-        iyy="0.011534"
-        iyz="0"
-        izz="0.000331" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://b2_description/meshes/RR_calf.dae" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 1.2 0" xyz="-0.002 0 -0.03"/>
-      <geometry>
-        <box size="0.1 0.0245 0.018"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 1.35 0" xyz="0.022 0 -0.1"/>
-      <geometry>
-        <box size="0.1 0.0245 0.018"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 1.53 0" xyz="0.036 0 -0.225"/>
-      <geometry>
-        <box size="0.15 0.0245 0.015"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 2 0" xyz="0.023 0 -0.31"/>
-      <geometry>
-        <box size="0.05 0.028 0.035"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="RR_calf_joint"
-    type="revolute">
-    <origin
-      xyz="0 8.6986E-05 -0.35"
-      rpy="0 0 0" />
-    <parent
-      link="RR_thigh" />
-    <child
-      link="RR_calf" />
-    <axis
-      xyz="0 1 0" />
-    <limit
-      lower="-2.82"
-      upper="-0.43"
-      effort="320"
-      velocity="14" />
-  </joint>
-  <link name="RR_calf_rotor">
-    <!-- <visual>
-      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.05"/>
-      </geometry>
-    </visual> -->
-    <collision>
-      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.05"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-      <mass value="0.2734"/>
-      <inertia ixx="0.000144463" ixy="0.0" ixz="0.0" iyy="0.000144463" iyz="0.0" izz="0.000263053"/>
-    </inertial>
-  </link>
-  <joint name="RR_calf_rotor_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0.05788 0"/>
-    <parent link="RR_thigh"/>
-    <child link="RR_calf_rotor"/>
-  </joint>
-  <link
-    name="RR_foot">
-    <inertial>
-      <origin
-        xyz="-0.006511 0 -0.010144"
-        rpy="0 0 0" />
-      <mass
-        value="0.126" />
-      <inertia
-        ixx="3.8E-05"
-        ixy="0"
-        ixz="1.1E-05"
-        iyy="4.1E-05"
-        iyz="0"
-        izz="4.2E-05" />
-    </inertial>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="1.57 0 0" />
-      <geometry>
-        <sphere radius="0.032" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="RR_foot_joint"
-    type="fixed">
-    <origin
-      xyz="0 -8.6984E-05 -0.35"
-      rpy="0 0 0" />
-    <parent
-      link="RR_calf" />
-    <child
-      link="RR_foot" />
-  </joint>
-  <link
-    name="f_oc_link">
-    <inertial>
-      <origin
-        xyz="-3.39E-15 1.46E-16 -6.11E-16"
-        rpy="0 0 0" />
-      <mass
-        value="4.19E-15" />
-      <inertia
-        ixx="1.68E-27"
-        ixy="-3.89E-44"
-        ixz="5.04E-44"
-        iyy="1.68E-27"
-        iyz="2.81E-45"
-        izz="1.68E-27" />
-    </inertial>
-    <!-- <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://b2_description/meshes/f_oc_link.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://b2_description/meshes/f_oc_link.STL" />
-      </geometry>
-    </collision> -->
-  </link>
-  <joint
-    name="joint_f_oc"
-    type="fixed">
-    <origin
-      xyz="0.39079 0 -0.013433"
-      rpy="-1.5708 0 -1.5708" />
-    <parent
-      link="base_link" />
-    <child
-      link="f_oc_link" />
-    <axis
-      xyz="0 0 0" />
-  </joint>
-  <link
-    name="r_oc_link">
-    <inertial>
-      <origin
-        xyz="-0.39143 1.52E-07 -0.011961"
-        rpy="0 0 0" />
-      <mass
-        value="4.19E-12" />
-      <inertia
-        ixx="1.68E-22"
-        ixy="9.03E-39"
-        ixz="0"
-        iyy="1.68E-22"
-        iyz="-1.2E-38"
-        izz="1.68E-22" />
-    </inertial>
-    <!-- <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://b2_description/meshes/r_oc_link.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://b2_description/meshes/r_oc_link.STL" />
-      </geometry>
-    </collision> -->
-  </link>
-  <joint
-    name="joint_r_oc"
-    type="fixed">
-    <origin
-      xyz="-0.38451 0 -0.023249"
-      rpy="-1.5708 0 1.5708" />
-    <parent
-      link="base_link" />
-    <child
-      link="r_oc_link" />
-    <axis
-      xyz="0 0 0" />
-  </joint>
-</robot>
+<?xml version="1.0" encoding="utf-8"?>
+<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) 
+     Commit Version: 1.6.0-4-g7f85cfe  Build Version: 1.6.7995.38578
+     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
+<robot
+  name="b2_description">
+  <link
+    name="base_link">
+    <inertial>
+      <origin
+        xyz="0.000458 0.005261 0.000665"
+        rpy="0 0 0" />
+      <mass
+        value="35.86" />
+      <inertia
+        ixx="0.27466"
+        ixy="-0.000622"
+        ixz="0.00315"
+        iyy="1.0618"
+        iyz="-0.00139"
+        izz="1.1825" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://b2_description/meshes/base_link.dae" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+        <geometry>
+          <box size="0.5 0.28 0.15" />
+        </geometry>
+    </collision>
+  </link>
+  <link
+    name="lidar_link">
+    <inertial>
+      <origin
+        xyz="0.34218 0 0.17851"
+        rpy="0 0 0" />
+      <mass
+        value="4.19E-15" />
+      <inertia
+        ixx="1.68E-27"
+        ixy="-3.2E-45"
+        ixz="7.68E-44"
+        iyy="1.68E-27"
+        iyz="-9.26E-47"
+        izz="1.68E-27" />
+    </inertial>
+    <collision>
+      <origin
+        xyz="0 0 -0.02"
+        rpy="0 0 0" />
+      <geometry>
+        <cylinder length="0.16" radius="0.076" />
+        <!-- <mesh
+          filename="package://b2_description/meshes/lidar_link.STL" /> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_lidar"
+    type="fixed">
+    <origin
+      xyz="0.34218 0 0.17851"
+      rpy="0 0 0" />
+    <parent
+      link="base_link" />
+    <child
+      link="lidar_link" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  <link
+    name="f_dc_link">
+    <inertial>
+      <origin
+        xyz="3.47E-18 0 5.55E-17"
+        rpy="0 0 0" />
+      <mass
+        value="4.19E-15" />
+      <inertia
+        ixx="1.68E-27"
+        ixy="-9.81E-45"
+        ixz="-2.54E-44"
+        iyy="1.68E-27"
+        iyz="0"
+        izz="1.68E-27" />
+    </inertial>
+  </link>
+  <joint
+    name="joint_f_dc"
+    type="fixed">
+    <origin
+      xyz="0.41251 0.024997 0.04765"
+      rpy="-2.3562 0 -1.5708" />
+    <parent
+      link="base_link" />
+    <child
+      link="f_dc_link" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  <link
+    name="r_dc_link">
+    <inertial>
+      <origin
+        xyz="-7.46E-16 -3.33E-16 -1.11E-15"
+        rpy="0 0 0" />
+      <mass
+        value="4.19E-15" />
+      <inertia
+        ixx="1.68E-27"
+        ixy="-4.31E-44"
+        ixz="2.48E-45"
+        iyy="1.68E-27"
+        iyz="0"
+        izz="1.68E-27" />
+    </inertial>
+  </link>
+  <joint
+    name="joint_r_dc"
+    type="fixed">
+    <origin
+      xyz="-0.4081 -0.025 0.024861"
+      rpy="-2.3557 0 1.5708" />
+    <parent
+      link="base_link" />
+    <child
+      link="r_dc_link" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  <link
+    name="imu_link">
+    <inertial>
+      <origin
+        xyz="-2.02E-17 1.73E-17 -1.39E-17"
+        rpy="0 0 0" />
+      <mass
+        value="4.19E-15" />
+      <inertia
+        ixx="1.68E-27"
+        ixy="1.84E-44"
+        ixz="-7.35E-44"
+        iyy="1.68E-27"
+        iyz="-3.67E-44"
+        izz="1.68E-27" />
+    </inertial>
+  </link>
+  <joint
+    name="joint_imu"
+    type="fixed">
+    <origin
+      xyz="0 -0.02341 0.04927"
+      rpy="0 0 0" />
+    <parent
+      link="base_link" />
+    <child
+      link="imu_link" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  <link
+    name="head_Link">
+    <inertial>
+      <origin
+        xyz="0.33989 -0.000168 0.12029"
+        rpy="0 0 0" />
+      <mass
+        value="3.6885" />
+      <inertia
+        ixx="0.026455"
+        ixy="3.15E-05"
+        ixz="0.00166"
+        iyy="0.029221"
+        iyz="-3.27E-05"
+        izz="0.010918" />
+    </inertial>
+    <collision>
+      <origin
+        xyz="0.41 0 0.005"
+        rpy="0 0 0" />
+      <geometry>
+        <box size="0.04 0.12 0.14" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_head"
+    type="fixed">
+    <origin
+      xyz="0 0 0"
+      rpy="0 0 0" />
+    <parent
+      link="base_link" />
+    <child
+      link="head_Link" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  <link
+    name="tail_link">
+    <inertial>
+      <origin
+        xyz="-0.37448 0.000488 0.006495"
+        rpy="0 0 0" />
+      <mass
+        value="0.795" />
+      <inertia
+        ixx="0.0028118"
+        ixy="1.95E-05"
+        ixz="-7.87E-06"
+        iyy="0.0047506"
+        iyz="-1.57E-05"
+        izz="0.0029154" />
+    </inertial>
+    <collision>
+      <origin
+        xyz="-0.405 0 0.005"
+        rpy="0 0 0" />
+      <geometry>
+        <box size="0.025 0.12 0.14" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_tail"
+    type="fixed">
+    <origin
+      xyz="0 0 0"
+      rpy="0 0 0" />
+    <parent
+      link="base_link" />
+    <child
+      link="tail_link" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  <link
+    name="FL_hip">
+    <inertial>
+      <origin
+        xyz="-0.003841 -0.009068 0"
+        rpy="0 0 0" />
+      <mass
+        value="2.673" />
+      <inertia
+        ixx="0.0033188"
+        ixy="7.16E-05"
+        ixz="-3.77E-07"
+        iyy="0.0048743"
+        iyz="4E-09"
+        izz="0.0037087" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://b2_description/meshes/FL_hip.dae" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0.12 0"
+        rpy="1.5707963267948966 0 0" />
+      <geometry>
+        <cylinder length="0.05" radius="0.07" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="FL_hip_joint"
+    type="revolute">
+    <origin
+      xyz="0.3285 0.072 0"
+      rpy="0 0 0" />
+    <parent
+      link="base_link" />
+    <child
+      link="FL_hip" />
+    <axis
+      xyz="1 0 0" />
+    <limit
+      lower="-0.87"
+      upper="0.87"
+      effort="200"
+      velocity="23" />
+  </joint>
+  <link name="FL_hip_rotor">
+    <!-- <visual>
+      <origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.05"/>
+      </geometry>
+    </visual> -->
+    <collision>
+      <origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.05"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <mass value="0.2734"/>
+      <inertia ixx="0.000144463" ixy="0.0" ixz="0.0" iyy="0.000144463" iyz="0.0" izz="0.000263053"/>
+    </inertial>
+  </link>
+  <joint name="FL_hip_rotor_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0.20205 0.072 0"/>
+    <parent link="base_link"/>
+    <child link="FL_hip_rotor"/>
+  </joint>
+  <link
+    name="FL_thigh">
+    <inertial>
+      <origin
+        xyz="-0.006279 -0.032049 -0.057835"
+        rpy="0 0 0" />
+      <mass
+        value="4.536" />
+      <inertia
+        ixx="0.062299"
+        ixy="0.00087781"
+        ixz="-0.0036475"
+        iyy="0.061399"
+        iyz="0.0083537"
+        izz="0.0081997" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://b2_description/meshes/FL_thigh.dae" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="-0.025 0 -0.16"
+        rpy="0 1.57 0" />
+      <geometry>
+        <box size="0.32 0.0445 0.054" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="FL_thigh_joint"
+    type="revolute">
+    <origin
+      xyz="0 0.11973 0"
+      rpy="0 0 0" />
+    <parent
+      link="FL_hip" />
+    <child
+      link="FL_thigh" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-0.94"
+      upper="4.69"
+      effort="200"
+      velocity="23" />
+  </joint>
+  <link name="FL_thigh_rotor">
+    <!-- <visual>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.05"/>
+      </geometry>
+    </visual> -->
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.05"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <mass value="0.2734"/>
+      <inertia ixx="0.000144463" ixy="0.0" ixz="0.0" iyy="0.000144463" iyz="0.0" izz="0.000263053"/>
+    </inertial>
+  </link>
+  <joint name="FL_thigh_rotor_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 -0.00798 0"/>
+    <parent link="FL_hip"/>
+    <child link="FL_thigh_rotor"/>
+  </joint>
+  <link
+    name="FL_calf">
+    <inertial>
+      <origin
+        xyz="0.012422 0 -0.12499"
+        rpy="0 0 0" />
+      <mass
+        value="0.404" />
+      <inertia
+        ixx="0.01143"
+        ixy="0"
+        ixz="0.000643"
+        iyy="0.011534"
+        iyz="0"
+        izz="0.000331" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://b2_description/meshes/FL_calf.dae" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 1.2 0" xyz="-0.002 0 -0.03"/>
+      <geometry>
+        <box size="0.1 0.0245 0.018"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 1.35 0" xyz="0.022 0 -0.1"/>
+      <geometry>
+        <box size="0.1 0.0245 0.018"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 1.53 0" xyz="0.036 0 -0.225"/>
+      <geometry>
+        <box size="0.15 0.0245 0.015"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 2 0" xyz="0.023 0 -0.31"/>
+      <geometry>
+        <box size="0.05 0.028 0.035"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="FL_calf_joint"
+    type="revolute">
+    <origin
+      xyz="0 -8.6984E-05 -0.35"
+      rpy="0 0 0" />
+    <parent
+      link="FL_thigh" />
+    <child
+      link="FL_calf" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-2.82"
+      upper="-0.43"
+      effort="320"
+      velocity="14" />
+  </joint>
+  <link name="FL_calf_rotor">
+    <!-- <visual>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.05"/>
+      </geometry>
+    </visual> -->
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.05"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <mass value="0.2734"/>
+      <inertia ixx="0.000144463" ixy="0.0" ixz="0.0" iyy="0.000144463" iyz="0.0" izz="0.000263053"/>
+    </inertial>
+  </link>
+  <joint name="FL_calf_rotor_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 -0.05788 0"/>
+    <parent link="FL_thigh"/>
+    <child link="FL_calf_rotor"/>
+  </joint>
+  <link
+    name="FL_foot">
+    <inertial>
+      <origin
+        xyz="-0.006511 0 -0.010144"
+        rpy="0 0 0" />
+      <mass
+        value="0.126" />
+      <inertia
+        ixx="3.8E-05"
+        ixy="0"
+        ixz="1.1E-05"
+        iyy="4.1E-05"
+        iyz="0"
+        izz="4.2E-05" />
+    </inertial>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="1.57 0 0" />
+      <geometry>
+        <sphere radius="0.032" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="FL_foot_joint"
+    type="fixed">
+    <origin
+      xyz="0 0 -0.35"
+      rpy="0 0 0" />
+    <parent
+      link="FL_calf" />
+    <child
+      link="FL_foot" />
+  </joint>
+  <link
+    name="FR_hip">
+    <inertial>
+      <origin
+        xyz="-0.009305 0.010228 0.000264"
+        rpy="0 0 0" />
+      <mass
+        value="2.673" />
+      <inertia
+        ixx="0.0026431"
+        ixy="-0.00019234"
+        ixz="-6.76E-06"
+        iyy="0.0046728"
+        iyz="7.16E-06"
+        izz="0.0034208" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://b2_description/meshes/FR_hip.dae" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 -0.12 0"
+        rpy="1.5707963267948966 0 0" />
+      <geometry>
+        <cylinder length="0.05" radius="0.07" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="FR_hip_joint"
+    type="revolute">
+    <origin
+      xyz="0.3285 -0.072 0"
+      rpy="0 0 0" />
+    <parent
+      link="base_link" />
+    <child
+      link="FR_hip" />
+    <axis
+      xyz="1 0 0" />
+    <limit
+      lower="-0.87"
+      upper="0.87"
+      effort="200"
+      velocity="23" />
+  </joint>
+  <link name="FR_hip_rotor">
+    <!-- <visual>
+      <origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.05"/>
+      </geometry>
+    </visual> -->
+    <collision>
+      <origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.05"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <mass value="0.2734"/>
+      <inertia ixx="0.000144463" ixy="0.0" ixz="0.0" iyy="0.000144463" iyz="0.0" izz="0.000263053"/>
+    </inertial>
+  </link>
+  <joint name="FR_hip_rotor_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0.20205 -0.072 0"/>
+    <parent link="base_link"/>
+    <child link="FR_hip_rotor"/>
+  </joint>
+  <link
+    name="FR_thigh">
+    <inertial>
+      <origin
+        xyz="-0.006279 0.032049 -0.057835"
+        rpy="0 0 0" />
+      <mass
+        value="4.536" />
+      <inertia
+        ixx="0.06229908"
+        ixy="-0.000877808"
+        ixz="-0.003647464"
+        iyy="0.06139927"
+        iyz="-0.008353671"
+        izz="0.008199745" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://b2_description/meshes/FR_thigh.dae" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 1.57 0" xyz="-0.03 0 -0.2"/>
+      <geometry>
+        <box size="0.25 0.028 0.04"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="FR_thigh_joint"
+    type="revolute">
+    <origin
+      xyz="0 -0.11973 0"
+      rpy="0 0 0" />
+    <parent
+      link="FR_hip" />
+    <child
+      link="FR_thigh" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-0.94"
+      upper="4.69"
+      effort="200"
+      velocity="23" />
+  </joint>
+  <link name="FR_thigh_rotor">
+    <!-- <visual>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.05"/>
+      </geometry>
+    </visual> -->
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.05"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <mass value="0.2734"/>
+      <inertia ixx="0.000144463" ixy="0.0" ixz="0.0" iyy="0.000144463" iyz="0.0" izz="0.000263053"/>
+    </inertial>
+  </link>
+  <joint name="FR_thigh_rotor_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0.00798 0"/>
+    <parent link="FR_hip"/>
+    <child link="FR_thigh_rotor"/>
+  </joint>
+  <link
+    name="FR_calf">
+    <inertial>
+      <origin
+        xyz="0.012422 0 -0.12499"
+        rpy="0 0 0" />
+      <mass
+        value="0.404" />
+      <inertia
+        ixx="0.01143"
+        ixy="0"
+        ixz="0.000643"
+        iyy="0.011534"
+        iyz="0"
+        izz="0.000331" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://b2_description/meshes/FR_calf.dae" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 1.2 0" xyz="-0.002 0 -0.03"/>
+      <geometry>
+        <box size="0.1 0.0245 0.018"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 1.35 0" xyz="0.022 0 -0.1"/>
+      <geometry>
+        <box size="0.1 0.0245 0.018"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 1.53 0" xyz="0.036 0 -0.225"/>
+      <geometry>
+        <box size="0.15 0.0245 0.015"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 2 0" xyz="0.023 0 -0.31"/>
+      <geometry>
+        <box size="0.05 0.028 0.035"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="FR_calf_joint"
+    type="revolute">
+    <origin
+      xyz="0 8.6986E-05 -0.35"
+      rpy="0 0 0" />
+    <parent
+      link="FR_thigh" />
+    <child
+      link="FR_calf" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-2.82"
+      upper="-0.43"
+      effort="320"
+      velocity="14" />
+  </joint>
+  <link name="FR_calf_rotor">
+    <!-- <visual>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.05"/>
+      </geometry>
+      <material name="green"/>
+    </visual> -->
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.05"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <mass value="0.2734"/>
+      <inertia ixx="0.000144463" ixy="0.0" ixz="0.0" iyy="0.000144463" iyz="0.0" izz="0.000263053"/>
+    </inertial>
+  </link>
+  <joint name="FR_calf_rotor_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0.05788 0"/>
+    <parent link="FR_thigh"/>
+    <child link="FR_calf_rotor"/>
+  </joint>
+  <link
+    name="FR_foot">
+    <inertial>
+      <origin
+        xyz="-0.006511 0 -0.010144"
+        rpy="0 0 0" />
+      <mass
+        value="0.126" />
+      <inertia
+        ixx="3.8E-05"
+        ixy="0"
+        ixz="1.1E-05"
+        iyy="4.1E-05"
+        iyz="0"
+        izz="4.2E-05" />
+    </inertial>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="1.57 0 0" />
+      <geometry>
+        <sphere radius="0.032" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="FR_foot_joint"
+    type="fixed">
+    <origin
+      xyz="0 0 -0.35"
+      rpy="0 0 0" />
+    <parent
+      link="FR_calf" />
+    <child
+      link="FR_foot" />
+  </joint>
+  <link
+    name="RL_hip">
+    <inertial>
+      <origin
+        xyz="0.009305 -0.010228 0.000264"
+        rpy="0 0 0" />
+      <mass
+        value="2.673" />
+      <inertia
+        ixx="0.0026431"
+        ixy="-0.00019234"
+        ixz="6.76E-06"
+        iyy="0.0046728"
+        iyz="-7.16E-06"
+        izz="0.0034208" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://b2_description/meshes/RL_hip.dae" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0.12 0"
+        rpy="1.5707963267948966 0 0" />
+      <geometry>
+        <cylinder length="0.05" radius="0.07" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="RL_hip_joint"
+    type="revolute">
+    <origin
+      xyz="-0.3285 0.072 0"
+      rpy="0 0 0" />
+    <parent
+      link="base_link" />
+    <child
+      link="RL_hip" />
+    <axis
+      xyz="1 0 0" />
+    <limit
+      lower="-0.87"
+      upper="0.87"
+      effort="200"
+      velocity="23" />
+  </joint>
+  <link name="RL_hip_rotor">
+    <!-- <visual>
+      <origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.05"/>
+      </geometry>
+    </visual> -->
+    <collision>
+      <origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.05"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <mass value="0.2734"/>
+      <inertia ixx="0.000144463" ixy="0.0" ixz="0.0" iyy="0.000144463" iyz="0.0" izz="0.000263053"/>
+    </inertial>
+  </link>
+  <joint name="RL_hip_rotor_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="-0.20205 0.072 0"/>
+    <parent link="base_link"/>
+    <child link="RL_hip_rotor"/>
+  </joint>
+  <link
+    name="RL_thigh">
+    <inertial>
+      <origin
+        xyz="-0.006279 -0.032049 -0.057835"
+        rpy="0 0 0" />
+      <mass
+        value="4.536" />
+      <inertia
+        ixx="0.06229908"
+        ixy="0.000877808"
+        ixz="-0.003647464"
+        iyy="0.06139927"
+        iyz="0.008353671"
+        izz="0.008199745" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://b2_description/meshes/RL_thigh.dae" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 1.57 0" xyz="-0.03 0 -0.2"/>
+      <geometry>
+        <box size="0.25 0.028 0.04"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="RL_thigh_joint"
+    type="revolute">
+    <origin
+      xyz="0 0.11973 0"
+      rpy="0 0 0" />
+    <parent
+      link="RL_hip" />
+    <child
+      link="RL_thigh" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-0.94"
+      upper="4.69"
+      effort="200"
+      velocity="23" />
+  </joint>
+  <link name="RL_thigh_rotor">
+    <!-- <visual>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.05"/>
+      </geometry>
+    </visual> -->
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.05"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <mass value="0.2734"/>
+      <inertia ixx="0.000144463" ixy="0.0" ixz="0.0" iyy="0.000144463" iyz="0.0" izz="0.000263053"/>
+    </inertial>
+  </link>
+  <joint name="RL_thigh_rotor_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 -0.00798 0"/>
+    <parent link="RL_hip"/>
+    <child link="RL_thigh_rotor"/>
+  </joint>
+  <link
+    name="RL_calf">
+    <inertial>
+      <origin
+        xyz="0.012422 0 -0.12499"
+        rpy="0 0 0" />
+      <mass
+        value="0.404" />
+      <inertia
+        ixx="0.01143"
+        ixy="0"
+        ixz="0.000643"
+        iyy="0.011534"
+        iyz="0"
+        izz="0.000331" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://b2_description/meshes/RL_calf.dae" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 1.2 0" xyz="-0.002 0 -0.03"/>
+      <geometry>
+        <box size="0.1 0.0245 0.018"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 1.35 0" xyz="0.022 0 -0.1"/>
+      <geometry>
+        <box size="0.1 0.0245 0.018"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 1.53 0" xyz="0.036 0 -0.225"/>
+      <geometry>
+        <box size="0.15 0.0245 0.015"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 2 0" xyz="0.023 0 -0.31"/>
+      <geometry>
+        <box size="0.05 0.028 0.035"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="RL_calf_joint"
+    type="revolute">
+    <origin
+      xyz="0 -8.6984E-05 -0.35"
+      rpy="0 0 0" />
+    <parent
+      link="RL_thigh" />
+    <child
+      link="RL_calf" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-2.82"
+      upper="-0.43"
+      effort="320"
+      velocity="14" />
+  </joint>
+  <link name="RL_calf_rotor">
+    <!-- <visual>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.05"/>
+      </geometry>
+    </visual> -->
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.05"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <mass value="0.2734"/>
+      <inertia ixx="0.000144463" ixy="0.0" ixz="0.0" iyy="0.000144463" iyz="0.0" izz="0.000263053"/>
+    </inertial>
+  </link>
+  <joint name="RL_calf_rotor_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 -0.05788 0"/>
+    <parent link="RL_thigh"/>
+    <child link="RL_calf_rotor"/>
+  </joint>
+  <link
+    name="RL_foot">
+    <inertial>
+      <origin
+        xyz="-0.006511 0 -0.010144"
+        rpy="0 0 0" />
+      <mass
+        value="0.126" />
+      <inertia
+        ixx="3.8E-05"
+        ixy="0"
+        ixz="1.1E-05"
+        iyy="4.1E-05"
+        iyz="0"
+        izz="4.2E-05" />
+    </inertial>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="1.57 0 0" />
+      <geometry>
+        <sphere radius="0.032" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="RL_foot_joint"
+    type="fixed">
+    <origin
+      xyz="0 0 -0.35"
+      rpy="0 0 0" />
+    <parent
+      link="RL_calf" />
+    <child
+      link="RL_foot" />
+  </joint>
+  <link
+    name="RR_hip">
+    <inertial>
+      <origin
+        xyz="0.003841 0.009068 0"
+        rpy="0 0 0" />
+      <mass
+        value="2.673" />
+      <inertia
+        ixx="0.0033188"
+        ixy="7.16E-05"
+        ixz="3.77E-07"
+        iyy="0.0048743"
+        iyz="-4E-09"
+        izz="0.0037087" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://b2_description/meshes/RR_hip.dae" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 -0.12 0"
+        rpy="1.5707963267948966 0 0" />
+      <geometry>
+        <cylinder length="0.05" radius="0.07" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="RR_hip_joint"
+    type="revolute">
+    <origin
+      xyz="-0.3285 -0.072 0"
+      rpy="0 0 0" />
+    <parent
+      link="base_link" />
+    <child
+      link="RR_hip" />
+    <axis
+      xyz="1 0 0" />
+    <limit
+      lower="-0.87"
+      upper="0.87"
+      effort="200"
+      velocity="23" />
+  </joint>
+  <link name="RR_hip_rotor">
+    <!-- <visual>
+      <origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.05"/>
+      </geometry>
+    </visual> -->
+    <collision>
+      <origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.05"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <mass value="0.2734"/>
+      <inertia ixx="0.000144463" ixy="0.0" ixz="0.0" iyy="0.000144463" iyz="0.0" izz="0.000263053"/>
+    </inertial>
+  </link>
+  <joint name="RR_hip_rotor_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="-0.20205 -0.072 0"/>
+    <parent link="base_link"/>
+    <child link="RR_hip_rotor"/>
+  </joint>
+  <link
+    name="RR_thigh">
+    <inertial>
+      <origin
+        xyz="-0.006279 0.032049 -0.057835"
+        rpy="0 0 0" />
+      <mass
+        value="4.536" />
+      <inertia
+        ixx="0.062299"
+        ixy="-0.00087781"
+        ixz="-0.0036475"
+        iyy="0.061399"
+        iyz="-0.0083537"
+        izz="0.0081997" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://b2_description/meshes/RR_thigh.dae" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 1.57 0" xyz="-0.03 0 -0.2"/>
+      <geometry>
+        <box size="0.25 0.028 0.04"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="RR_thigh_joint"
+    type="revolute">
+    <origin
+      xyz="0 -0.11973 0"
+      rpy="0 0 0" />
+    <parent
+      link="RR_hip" />
+    <child
+      link="RR_thigh" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-0.94"
+      upper="4.69"
+      effort="200"
+      velocity="23" />
+  </joint>
+  <link name="RR_thigh_rotor">
+    <!-- <visual>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.05"/>
+      </geometry>
+    </visual> -->
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.05"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <mass value="0.2734"/>
+      <inertia ixx="0.000144463" ixy="0.0" ixz="0.0" iyy="0.000144463" iyz="0.0" izz="0.000263053"/>
+    </inertial>
+  </link>
+  <joint name="RR_thigh_rotor_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0.00798 0"/>
+    <parent link="RR_hip"/>
+    <child link="RR_thigh_rotor"/>
+  </joint>
+  <link
+    name="RR_calf">
+    <inertial>
+      <origin
+        xyz="0.012422 0 -0.12499"
+        rpy="0 0 0" />
+      <mass
+        value="0.404" />
+      <inertia
+        ixx="0.01143"
+        ixy="0"
+        ixz="0.000643"
+        iyy="0.011534"
+        iyz="0"
+        izz="0.000331" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://b2_description/meshes/RR_calf.dae" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 1.2 0" xyz="-0.002 0 -0.03"/>
+      <geometry>
+        <box size="0.1 0.0245 0.018"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 1.35 0" xyz="0.022 0 -0.1"/>
+      <geometry>
+        <box size="0.1 0.0245 0.018"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 1.53 0" xyz="0.036 0 -0.225"/>
+      <geometry>
+        <box size="0.15 0.0245 0.015"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 2 0" xyz="0.023 0 -0.31"/>
+      <geometry>
+        <box size="0.05 0.028 0.035"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="RR_calf_joint"
+    type="revolute">
+    <origin
+      xyz="0 8.6986E-05 -0.35"
+      rpy="0 0 0" />
+    <parent
+      link="RR_thigh" />
+    <child
+      link="RR_calf" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-2.82"
+      upper="-0.43"
+      effort="320"
+      velocity="14" />
+  </joint>
+  <link name="RR_calf_rotor">
+    <!-- <visual>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.05"/>
+      </geometry>
+    </visual> -->
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.05"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <mass value="0.2734"/>
+      <inertia ixx="0.000144463" ixy="0.0" ixz="0.0" iyy="0.000144463" iyz="0.0" izz="0.000263053"/>
+    </inertial>
+  </link>
+  <joint name="RR_calf_rotor_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0.05788 0"/>
+    <parent link="RR_thigh"/>
+    <child link="RR_calf_rotor"/>
+  </joint>
+  <link
+    name="RR_foot">
+    <inertial>
+      <origin
+        xyz="-0.006511 0 -0.010144"
+        rpy="0 0 0" />
+      <mass
+        value="0.126" />
+      <inertia
+        ixx="3.8E-05"
+        ixy="0"
+        ixz="1.1E-05"
+        iyy="4.1E-05"
+        iyz="0"
+        izz="4.2E-05" />
+    </inertial>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="1.57 0 0" />
+      <geometry>
+        <sphere radius="0.032" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="RR_foot_joint"
+    type="fixed">
+    <origin
+      xyz="0 -8.6984E-05 -0.35"
+      rpy="0 0 0" />
+    <parent
+      link="RR_calf" />
+    <child
+      link="RR_foot" />
+  </joint>
+  <link
+    name="f_oc_link">
+    <inertial>
+      <origin
+        xyz="-3.39E-15 1.46E-16 -6.11E-16"
+        rpy="0 0 0" />
+      <mass
+        value="4.19E-15" />
+      <inertia
+        ixx="1.68E-27"
+        ixy="-3.89E-44"
+        ixz="5.04E-44"
+        iyy="1.68E-27"
+        iyz="2.81E-45"
+        izz="1.68E-27" />
+    </inertial>
+    <!-- <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://b2_description/meshes/f_oc_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://b2_description/meshes/f_oc_link.STL" />
+      </geometry>
+    </collision> -->
+  </link>
+  <joint
+    name="joint_f_oc"
+    type="fixed">
+    <origin
+      xyz="0.39079 0 -0.013433"
+      rpy="-1.5708 0 -1.5708" />
+    <parent
+      link="base_link" />
+    <child
+      link="f_oc_link" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  <link
+    name="r_oc_link">
+    <inertial>
+      <origin
+        xyz="-0.39143 1.52E-07 -0.011961"
+        rpy="0 0 0" />
+      <mass
+        value="4.19E-12" />
+      <inertia
+        ixx="1.68E-22"
+        ixy="9.03E-39"
+        ixz="0"
+        iyy="1.68E-22"
+        iyz="-1.2E-38"
+        izz="1.68E-22" />
+    </inertial>
+    <!-- <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://b2_description/meshes/r_oc_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://b2_description/meshes/r_oc_link.STL" />
+      </geometry>
+    </collision> -->
+  </link>
+  <joint
+    name="joint_r_oc"
+    type="fixed">
+    <origin
+      xyz="-0.38451 0 -0.023249"
+      rpy="-1.5708 0 1.5708" />
+    <parent
+      link="base_link" />
+    <child
+      link="r_oc_link" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+</robot>
diff --git a/descriptions/b2_description/xacro/const.xacro b/descriptions/unitree/b2_description/xacro/const.xacro
similarity index 100%
rename from descriptions/b2_description/xacro/const.xacro
rename to descriptions/unitree/b2_description/xacro/const.xacro
diff --git a/descriptions/b2_description/xacro/gazebo.xacro b/descriptions/unitree/b2_description/xacro/gazebo.xacro
similarity index 100%
rename from descriptions/b2_description/xacro/gazebo.xacro
rename to descriptions/unitree/b2_description/xacro/gazebo.xacro
diff --git a/descriptions/b2_description/xacro/leg.xacro b/descriptions/unitree/b2_description/xacro/leg.xacro
similarity index 100%
rename from descriptions/b2_description/xacro/leg.xacro
rename to descriptions/unitree/b2_description/xacro/leg.xacro
diff --git a/descriptions/b2_description/xacro/materials.xacro b/descriptions/unitree/b2_description/xacro/materials.xacro
similarity index 100%
rename from descriptions/b2_description/xacro/materials.xacro
rename to descriptions/unitree/b2_description/xacro/materials.xacro
diff --git a/descriptions/b2_description/xacro/robot.xacro b/descriptions/unitree/b2_description/xacro/robot.xacro
similarity index 100%
rename from descriptions/b2_description/xacro/robot.xacro
rename to descriptions/unitree/b2_description/xacro/robot.xacro
diff --git a/descriptions/b2_description/xacro/ros2_control.xacro b/descriptions/unitree/b2_description/xacro/ros2_control.xacro
similarity index 100%
rename from descriptions/b2_description/xacro/ros2_control.xacro
rename to descriptions/unitree/b2_description/xacro/ros2_control.xacro
diff --git a/descriptions/b2_description/xacro/stairs.xacro b/descriptions/unitree/b2_description/xacro/stairs.xacro
similarity index 100%
rename from descriptions/b2_description/xacro/stairs.xacro
rename to descriptions/unitree/b2_description/xacro/stairs.xacro
diff --git a/descriptions/b2_description/xacro/transmission.xacro b/descriptions/unitree/b2_description/xacro/transmission.xacro
similarity index 100%
rename from descriptions/b2_description/xacro/transmission.xacro
rename to descriptions/unitree/b2_description/xacro/transmission.xacro
diff --git a/descriptions/go1_description/CMakeLists.txt b/descriptions/unitree/go1_description/CMakeLists.txt
similarity index 100%
rename from descriptions/go1_description/CMakeLists.txt
rename to descriptions/unitree/go1_description/CMakeLists.txt
diff --git a/descriptions/go1_description/README.md b/descriptions/unitree/go1_description/README.md
similarity index 95%
rename from descriptions/go1_description/README.md
rename to descriptions/unitree/go1_description/README.md
index c2435cc..67308e8 100644
--- a/descriptions/go1_description/README.md
+++ b/descriptions/unitree/go1_description/README.md
@@ -1,7 +1,7 @@
 # Unitree Go1 Description
 This repository contains the urdf model of go1.
 
-![go1](../../.images/go1.png)
+![go1](../../../.images/go1.png)
 
 ## Build
 ```bash
diff --git a/descriptions/go1_description/config/ocs2/gait.info b/descriptions/unitree/go1_description/config/ocs2/gait.info
similarity index 100%
rename from descriptions/go1_description/config/ocs2/gait.info
rename to descriptions/unitree/go1_description/config/ocs2/gait.info
diff --git a/descriptions/go1_description/config/ocs2/reference.info b/descriptions/unitree/go1_description/config/ocs2/reference.info
similarity index 60%
rename from descriptions/go1_description/config/ocs2/reference.info
rename to descriptions/unitree/go1_description/config/ocs2/reference.info
index 36aa1eb..accc9bf 100644
--- a/descriptions/go1_description/config/ocs2/reference.info
+++ b/descriptions/unitree/go1_description/config/ocs2/reference.info
@@ -1,21 +1,21 @@
 targetDisplacementVelocity          0.5;
 targetRotationVelocity              1.57;
 
-comHeight                           0.3
+comHeight                           0.307
 
 defaultJointState
 {
-   (0,0)  -0.10   ; FL_hip_joint
-   (1,0)   0.72   ; FL_thigh_joint
+   (0,0)  -0.20   ; FL_hip_joint
+   (1,0)   0.7   ; FL_thigh_joint
    (2,0)  -1.44   ; FL_calf_joint
-   (3,0)   0.10   ; FR_hip_joint
-   (4,0)   0.72   ; FR_thigh_joint
+   (3,0)   0.20   ; FR_hip_joint
+   (4,0)   0.7   ; FR_thigh_joint
    (5,0)  -1.44   ; FR_calf_joint
-   (6,0)  -0.10   ; RL_hip_joint
-   (7,0)   0.72   ; RL_thigh_joint
+   (6,0)  -0.20   ; RL_hip_joint
+   (7,0)   0.7   ; RL_thigh_joint
    (8,0)  -1.44   ; RL_calf_joint
-   (9,0)   0.10   ; RR_hip_joint
-   (10,0)  0.72   ; RR_thigh_joint
+   (9,0)   0.20   ; RR_hip_joint
+   (10,0)  0.7   ; RR_thigh_joint
    (11,0) -1.44   ; RR_calf_joint
 }
 
diff --git a/descriptions/go1_description/config/ocs2/task.info b/descriptions/unitree/go1_description/config/ocs2/task.info
similarity index 100%
rename from descriptions/go1_description/config/ocs2/task.info
rename to descriptions/unitree/go1_description/config/ocs2/task.info
diff --git a/descriptions/go1_description/config/robot_control.yaml b/descriptions/unitree/go1_description/config/robot_control.yaml
similarity index 100%
rename from descriptions/go1_description/config/robot_control.yaml
rename to descriptions/unitree/go1_description/config/robot_control.yaml
diff --git a/descriptions/go1_description/config/visualize_urdf.rviz b/descriptions/unitree/go1_description/config/visualize_urdf.rviz
similarity index 100%
rename from descriptions/go1_description/config/visualize_urdf.rviz
rename to descriptions/unitree/go1_description/config/visualize_urdf.rviz
diff --git a/descriptions/go1_description/launch/ocs2_control.launch.py b/descriptions/unitree/go1_description/launch/ocs2_control.launch.py
similarity index 100%
rename from descriptions/go1_description/launch/ocs2_control.launch.py
rename to descriptions/unitree/go1_description/launch/ocs2_control.launch.py
diff --git a/descriptions/go1_description/launch/unitree_guide.launch.py b/descriptions/unitree/go1_description/launch/unitree_guide.launch.py
similarity index 100%
rename from descriptions/go1_description/launch/unitree_guide.launch.py
rename to descriptions/unitree/go1_description/launch/unitree_guide.launch.py
diff --git a/descriptions/go1_description/launch/visualize.launch.py b/descriptions/unitree/go1_description/launch/visualize.launch.py
similarity index 100%
rename from descriptions/go1_description/launch/visualize.launch.py
rename to descriptions/unitree/go1_description/launch/visualize.launch.py
diff --git a/descriptions/go1_description/meshes/calf.dae b/descriptions/unitree/go1_description/meshes/calf.dae
similarity index 100%
rename from descriptions/go1_description/meshes/calf.dae
rename to descriptions/unitree/go1_description/meshes/calf.dae
diff --git a/descriptions/go1_description/meshes/depthCamera.dae b/descriptions/unitree/go1_description/meshes/depthCamera.dae
similarity index 100%
rename from descriptions/go1_description/meshes/depthCamera.dae
rename to descriptions/unitree/go1_description/meshes/depthCamera.dae
diff --git a/descriptions/go1_description/meshes/hip.dae b/descriptions/unitree/go1_description/meshes/hip.dae
similarity index 100%
rename from descriptions/go1_description/meshes/hip.dae
rename to descriptions/unitree/go1_description/meshes/hip.dae
diff --git a/descriptions/go1_description/meshes/thigh.dae b/descriptions/unitree/go1_description/meshes/thigh.dae
similarity index 100%
rename from descriptions/go1_description/meshes/thigh.dae
rename to descriptions/unitree/go1_description/meshes/thigh.dae
diff --git a/descriptions/go1_description/meshes/thigh_mirror.dae b/descriptions/unitree/go1_description/meshes/thigh_mirror.dae
similarity index 100%
rename from descriptions/go1_description/meshes/thigh_mirror.dae
rename to descriptions/unitree/go1_description/meshes/thigh_mirror.dae
diff --git a/descriptions/go1_description/meshes/trunk.dae b/descriptions/unitree/go1_description/meshes/trunk.dae
similarity index 100%
rename from descriptions/go1_description/meshes/trunk.dae
rename to descriptions/unitree/go1_description/meshes/trunk.dae
diff --git a/descriptions/go1_description/meshes/ultraSound.dae b/descriptions/unitree/go1_description/meshes/ultraSound.dae
similarity index 100%
rename from descriptions/go1_description/meshes/ultraSound.dae
rename to descriptions/unitree/go1_description/meshes/ultraSound.dae
diff --git a/descriptions/go1_description/package.xml b/descriptions/unitree/go1_description/package.xml
similarity index 100%
rename from descriptions/go1_description/package.xml
rename to descriptions/unitree/go1_description/package.xml
diff --git a/descriptions/go1_description/urdf/robot.urdf b/descriptions/unitree/go1_description/urdf/robot.urdf
similarity index 100%
rename from descriptions/go1_description/urdf/robot.urdf
rename to descriptions/unitree/go1_description/urdf/robot.urdf
diff --git a/descriptions/go1_description/xacro/const.xacro b/descriptions/unitree/go1_description/xacro/const.xacro
similarity index 100%
rename from descriptions/go1_description/xacro/const.xacro
rename to descriptions/unitree/go1_description/xacro/const.xacro
diff --git a/descriptions/go1_description/xacro/depthCamera.xacro b/descriptions/unitree/go1_description/xacro/depthCamera.xacro
similarity index 100%
rename from descriptions/go1_description/xacro/depthCamera.xacro
rename to descriptions/unitree/go1_description/xacro/depthCamera.xacro
diff --git a/descriptions/go1_description/xacro/gazebo.xacro b/descriptions/unitree/go1_description/xacro/gazebo.xacro
similarity index 100%
rename from descriptions/go1_description/xacro/gazebo.xacro
rename to descriptions/unitree/go1_description/xacro/gazebo.xacro
diff --git a/descriptions/go1_description/xacro/leg.xacro b/descriptions/unitree/go1_description/xacro/leg.xacro
similarity index 100%
rename from descriptions/go1_description/xacro/leg.xacro
rename to descriptions/unitree/go1_description/xacro/leg.xacro
diff --git a/descriptions/go1_description/xacro/materials.xacro b/descriptions/unitree/go1_description/xacro/materials.xacro
similarity index 100%
rename from descriptions/go1_description/xacro/materials.xacro
rename to descriptions/unitree/go1_description/xacro/materials.xacro
diff --git a/descriptions/go1_description/xacro/robot.xacro b/descriptions/unitree/go1_description/xacro/robot.xacro
similarity index 100%
rename from descriptions/go1_description/xacro/robot.xacro
rename to descriptions/unitree/go1_description/xacro/robot.xacro
diff --git a/descriptions/go1_description/xacro/ros2_control.xacro b/descriptions/unitree/go1_description/xacro/ros2_control.xacro
similarity index 100%
rename from descriptions/go1_description/xacro/ros2_control.xacro
rename to descriptions/unitree/go1_description/xacro/ros2_control.xacro
diff --git a/descriptions/go1_description/xacro/transmission.xacro b/descriptions/unitree/go1_description/xacro/transmission.xacro
similarity index 100%
rename from descriptions/go1_description/xacro/transmission.xacro
rename to descriptions/unitree/go1_description/xacro/transmission.xacro
diff --git a/descriptions/go1_description/xacro/ultraSound.xacro b/descriptions/unitree/go1_description/xacro/ultraSound.xacro
similarity index 100%
rename from descriptions/go1_description/xacro/ultraSound.xacro
rename to descriptions/unitree/go1_description/xacro/ultraSound.xacro
diff --git a/descriptions/go2_description/CMakeLists.txt b/descriptions/unitree/go2_description/CMakeLists.txt
similarity index 100%
rename from descriptions/go2_description/CMakeLists.txt
rename to descriptions/unitree/go2_description/CMakeLists.txt
diff --git a/descriptions/go2_description/README.md b/descriptions/unitree/go2_description/README.md
similarity index 97%
rename from descriptions/go2_description/README.md
rename to descriptions/unitree/go2_description/README.md
index 42a11de..10e2bb3 100644
--- a/descriptions/go2_description/README.md
+++ b/descriptions/unitree/go2_description/README.md
@@ -1,7 +1,7 @@
 # Unitree Go2 Description
 This repository contains the urdf model of go2.
 
-![go2](../../.images/go2.png)
+![go2](../../../.images/go2.png)
 
 Tested environment:
 * Ubuntu 24.04
diff --git a/descriptions/go2_description/config/ocs2/gait.info b/descriptions/unitree/go2_description/config/ocs2/gait.info
similarity index 100%
rename from descriptions/go2_description/config/ocs2/gait.info
rename to descriptions/unitree/go2_description/config/ocs2/gait.info
diff --git a/descriptions/go2_description/config/ocs2/reference.info b/descriptions/unitree/go2_description/config/ocs2/reference.info
similarity index 100%
rename from descriptions/go2_description/config/ocs2/reference.info
rename to descriptions/unitree/go2_description/config/ocs2/reference.info
diff --git a/descriptions/go2_description/config/ocs2/task.info b/descriptions/unitree/go2_description/config/ocs2/task.info
similarity index 100%
rename from descriptions/go2_description/config/ocs2/task.info
rename to descriptions/unitree/go2_description/config/ocs2/task.info
diff --git a/descriptions/go2_description/config/robot_control.yaml b/descriptions/unitree/go2_description/config/robot_control.yaml
similarity index 100%
rename from descriptions/go2_description/config/robot_control.yaml
rename to descriptions/unitree/go2_description/config/robot_control.yaml
diff --git a/descriptions/go2_description/config/visualize_urdf.rviz b/descriptions/unitree/go2_description/config/visualize_urdf.rviz
similarity index 100%
rename from descriptions/go2_description/config/visualize_urdf.rviz
rename to descriptions/unitree/go2_description/config/visualize_urdf.rviz
diff --git a/descriptions/go2_description/launch/ocs2_control.launch.py b/descriptions/unitree/go2_description/launch/ocs2_control.launch.py
similarity index 100%
rename from descriptions/go2_description/launch/ocs2_control.launch.py
rename to descriptions/unitree/go2_description/launch/ocs2_control.launch.py
diff --git a/descriptions/go2_description/launch/unitree_guide.launch.py b/descriptions/unitree/go2_description/launch/unitree_guide.launch.py
similarity index 100%
rename from descriptions/go2_description/launch/unitree_guide.launch.py
rename to descriptions/unitree/go2_description/launch/unitree_guide.launch.py
diff --git a/descriptions/go2_description/launch/visualize.launch.py b/descriptions/unitree/go2_description/launch/visualize.launch.py
similarity index 100%
rename from descriptions/go2_description/launch/visualize.launch.py
rename to descriptions/unitree/go2_description/launch/visualize.launch.py
diff --git a/descriptions/go2_description/meshes/calf.dae b/descriptions/unitree/go2_description/meshes/calf.dae
similarity index 100%
rename from descriptions/go2_description/meshes/calf.dae
rename to descriptions/unitree/go2_description/meshes/calf.dae
diff --git a/descriptions/go2_description/meshes/calf_mirror.dae b/descriptions/unitree/go2_description/meshes/calf_mirror.dae
similarity index 100%
rename from descriptions/go2_description/meshes/calf_mirror.dae
rename to descriptions/unitree/go2_description/meshes/calf_mirror.dae
diff --git a/descriptions/go2_description/meshes/foot.dae b/descriptions/unitree/go2_description/meshes/foot.dae
similarity index 100%
rename from descriptions/go2_description/meshes/foot.dae
rename to descriptions/unitree/go2_description/meshes/foot.dae
diff --git a/descriptions/go2_description/meshes/hip.dae b/descriptions/unitree/go2_description/meshes/hip.dae
similarity index 100%
rename from descriptions/go2_description/meshes/hip.dae
rename to descriptions/unitree/go2_description/meshes/hip.dae
diff --git a/descriptions/go2_description/meshes/thigh.dae b/descriptions/unitree/go2_description/meshes/thigh.dae
similarity index 100%
rename from descriptions/go2_description/meshes/thigh.dae
rename to descriptions/unitree/go2_description/meshes/thigh.dae
diff --git a/descriptions/go2_description/meshes/thigh_mirror.dae b/descriptions/unitree/go2_description/meshes/thigh_mirror.dae
similarity index 100%
rename from descriptions/go2_description/meshes/thigh_mirror.dae
rename to descriptions/unitree/go2_description/meshes/thigh_mirror.dae
diff --git a/descriptions/go2_description/meshes/trunk.dae b/descriptions/unitree/go2_description/meshes/trunk.dae
similarity index 100%
rename from descriptions/go2_description/meshes/trunk.dae
rename to descriptions/unitree/go2_description/meshes/trunk.dae
diff --git a/descriptions/go2_description/package.xml b/descriptions/unitree/go2_description/package.xml
similarity index 100%
rename from descriptions/go2_description/package.xml
rename to descriptions/unitree/go2_description/package.xml
diff --git a/descriptions/go2_description/urdf/robot.urdf b/descriptions/unitree/go2_description/urdf/robot.urdf
similarity index 100%
rename from descriptions/go2_description/urdf/robot.urdf
rename to descriptions/unitree/go2_description/urdf/robot.urdf
diff --git a/descriptions/go2_description/xacro/const.xacro b/descriptions/unitree/go2_description/xacro/const.xacro
similarity index 100%
rename from descriptions/go2_description/xacro/const.xacro
rename to descriptions/unitree/go2_description/xacro/const.xacro
diff --git a/descriptions/go2_description/xacro/gazebo.xacro b/descriptions/unitree/go2_description/xacro/gazebo.xacro
similarity index 100%
rename from descriptions/go2_description/xacro/gazebo.xacro
rename to descriptions/unitree/go2_description/xacro/gazebo.xacro
diff --git a/descriptions/go2_description/xacro/leg.xacro b/descriptions/unitree/go2_description/xacro/leg.xacro
similarity index 100%
rename from descriptions/go2_description/xacro/leg.xacro
rename to descriptions/unitree/go2_description/xacro/leg.xacro
diff --git a/descriptions/go2_description/xacro/materials.xacro b/descriptions/unitree/go2_description/xacro/materials.xacro
similarity index 100%
rename from descriptions/go2_description/xacro/materials.xacro
rename to descriptions/unitree/go2_description/xacro/materials.xacro
diff --git a/descriptions/go2_description/xacro/robot.xacro b/descriptions/unitree/go2_description/xacro/robot.xacro
similarity index 100%
rename from descriptions/go2_description/xacro/robot.xacro
rename to descriptions/unitree/go2_description/xacro/robot.xacro
diff --git a/descriptions/go2_description/xacro/ros2_control.xacro b/descriptions/unitree/go2_description/xacro/ros2_control.xacro
similarity index 100%
rename from descriptions/go2_description/xacro/ros2_control.xacro
rename to descriptions/unitree/go2_description/xacro/ros2_control.xacro
diff --git a/descriptions/go2_description/xacro/transmission.xacro b/descriptions/unitree/go2_description/xacro/transmission.xacro
similarity index 100%
rename from descriptions/go2_description/xacro/transmission.xacro
rename to descriptions/unitree/go2_description/xacro/transmission.xacro
diff --git a/descriptions/cyberdog_description/CMakeLists.txt b/descriptions/xiaomi/cyberdog_description/CMakeLists.txt
similarity index 77%
rename from descriptions/cyberdog_description/CMakeLists.txt
rename to descriptions/xiaomi/cyberdog_description/CMakeLists.txt
index 063155c..1091ea9 100644
--- a/descriptions/cyberdog_description/CMakeLists.txt
+++ b/descriptions/xiaomi/cyberdog_description/CMakeLists.txt
@@ -4,7 +4,7 @@ project(cyberdog_description)
 find_package(ament_cmake REQUIRED)
 
 install(
-        DIRECTORY meshes xacro launch config
+        DIRECTORY meshes xacro launch config urdf
         DESTINATION share/${PROJECT_NAME}/
 )
 
diff --git a/descriptions/cyberdog_description/README.md b/descriptions/xiaomi/cyberdog_description/README.md
similarity index 66%
rename from descriptions/cyberdog_description/README.md
rename to descriptions/xiaomi/cyberdog_description/README.md
index da80b9d..a49d3eb 100644
--- a/descriptions/cyberdog_description/README.md
+++ b/descriptions/xiaomi/cyberdog_description/README.md
@@ -19,4 +19,16 @@ ros2 launch cyberdog_description visualize.launch.py
 ```bash
 source ~/ros2_ws/install/setup.bash
 ros2 launch cyberdog_description gazebo.launch.py
-```
\ No newline at end of file
+```
+
+## Launch ROS2 Control
+* Unitree Guide Controller
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch cyberdog_description unitree_guide.launch.py
+  ```
+* OCS2 Quadruped Controller
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch cyberdog_description ocs2_control.launch.py
+  ```
diff --git a/descriptions/cyberdog_description/config/cyberdog.rviz b/descriptions/xiaomi/cyberdog_description/config/cyberdog.rviz
similarity index 100%
rename from descriptions/cyberdog_description/config/cyberdog.rviz
rename to descriptions/xiaomi/cyberdog_description/config/cyberdog.rviz
diff --git a/descriptions/xiaomi/cyberdog_description/config/ocs2/gait.info b/descriptions/xiaomi/cyberdog_description/config/ocs2/gait.info
new file mode 100644
index 0000000..6ea163b
--- /dev/null
+++ b/descriptions/xiaomi/cyberdog_description/config/ocs2/gait.info
@@ -0,0 +1,255 @@
+list
+{
+  [0] stance
+  [1] trot
+  [2] standing_trot
+  [3] flying_trot
+  [4] pace
+  [5] standing_pace
+  [6] dynamic_walk
+  [7] static_walk
+  [8] amble
+  [9] lindyhop
+  [10] skipping
+  [11] pawup
+}
+
+stance
+ {
+   modeSequence
+   {
+     [0]     STANCE
+   }
+   switchingTimes
+   {
+     [0]     0.0
+     [1]     0.5
+   }
+}
+
+trot
+ {
+   modeSequence
+   {
+     [0]     LF_RH
+     [1]     RF_LH
+   }
+   switchingTimes
+   {
+     [0]     0.0
+     [1]     0.3
+     [2]     0.6
+   }
+}
+
+standing_trot
+{
+  modeSequence
+  {
+    [0]     LF_RH
+    [1]     STANCE
+    [2]     RF_LH
+    [3]     STANCE
+  }
+  switchingTimes
+  {
+    [0]     0.00
+    [1]     0.25
+    [2]     0.3
+    [3]     0.55
+    [4]     0.6
+  }
+}
+
+flying_trot
+{
+  modeSequence
+  {
+    [0]     LF_RH
+    [1]     FLY
+    [2]     RF_LH
+    [3]     FLY
+  }
+  switchingTimes
+  {
+    [0]     0.00
+    [1]     0.15
+    [2]     0.2
+    [3]     0.35
+    [4]     0.4
+  }
+}
+
+pace
+{
+  modeSequence
+  {
+    [0]     LF_LH
+    [1]     FLY
+    [2]     RF_RH
+    [3]     FLY
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.28
+    [2]     0.30
+    [3]     0.58
+    [4]     0.60
+  }
+}
+
+standing_pace
+{
+  modeSequence
+  {
+    [0]     LF_LH
+    [1]     STANCE
+    [2]     RF_RH
+    [3]     STANCE
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.30
+    [2]     0.35
+    [3]     0.65
+    [4]     0.70
+  }
+}
+
+dynamic_walk
+{
+  modeSequence
+  {
+    [0]     LF_RF_RH
+    [1]     RF_RH
+    [2]     RF_LH_RH
+    [3]     LF_RF_LH
+    [4]     LF_LH
+    [5]     LF_LH_RH
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.2
+    [2]     0.3
+    [3]     0.5
+    [4]     0.7
+    [5]     0.8
+    [6]     1.0
+  }
+}
+
+static_walk
+{
+  modeSequence
+  {
+    [0]     LF_RF_RH
+    [1]     RF_LH_RH
+    [2]     LF_RF_LH
+    [3]     LF_LH_RH
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.3
+    [2]     0.6
+    [3]     0.9
+    [4]     1.2
+  }
+}
+
+amble
+{
+  modeSequence
+  {
+    [0]     RF_LH
+    [1]     LF_LH
+    [2]     LF_RH
+    [3]     RF_RH
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.15
+    [2]     0.40
+    [3]     0.55
+    [4]     0.80
+  }
+}
+
+lindyhop
+{
+  modeSequence
+  {
+    [0]     LF_RH
+    [1]     STANCE
+    [2]     RF_LH
+    [3]     STANCE
+    [4]     LF_LH
+    [5]     RF_RH
+    [6]     LF_LH
+    [7]     STANCE
+    [8]     RF_RH
+    [9]     LF_LH
+    [10]     RF_RH
+    [11]     STANCE
+  }
+  switchingTimes
+  {
+    [0]     0.00 ; Step 1
+    [1]     0.35 ; Stance
+    [2]     0.45 ; Step 2
+    [3]     0.80 ; Stance
+    [4]     0.90  ; Tripple step
+    [5]     1.125 ;
+    [6]     1.35  ;
+    [7]     1.70  ; Stance
+    [8]     1.80  ; Tripple step
+    [9]     2.025 ;
+    [10]    2.25  ;
+    [11]    2.60  ; Stance
+    [12]    2.70  ;
+  }
+}
+
+skipping
+{
+  modeSequence
+  {
+    [0]     LF_RH
+    [1]     FLY
+    [2]     LF_RH
+    [3]     FLY
+    [4]     RF_LH
+    [5]     FLY
+    [6]     RF_LH
+    [7]     FLY
+  }
+  switchingTimes
+  {
+    [0]     0.00
+    [1]     0.27
+    [2]     0.30
+    [3]     0.57
+    [4]     0.60
+    [5]     0.87
+    [6]     0.90
+    [7]     1.17
+    [8]     1.20
+  }
+}
+
+pawup
+{
+   modeSequence
+   {
+     [0]     RF_LH_RH
+   }
+   switchingTimes
+   {
+     [0]     0.0
+     [1]     2.0
+   }
+}
diff --git a/descriptions/xiaomi/cyberdog_description/config/ocs2/reference.info b/descriptions/xiaomi/cyberdog_description/config/ocs2/reference.info
new file mode 100644
index 0000000..47047e2
--- /dev/null
+++ b/descriptions/xiaomi/cyberdog_description/config/ocs2/reference.info
@@ -0,0 +1,46 @@
+targetDisplacementVelocity          0.4;
+targetRotationVelocity              1.57;
+
+comHeight                           0.3
+
+defaultJointState
+{
+      (0,0)   -0.10   ; FL_hip_joint
+      (1,0)   0.72   ; FL_thigh_joint
+      (2,0)  -1.44   ; FL_calf_joint
+      (3,0)   0.10   ; FR_hip_joint
+      (4,0)   0.72   ; FR_thigh_joint
+      (5,0)  -1.44   ; FR_calf_joint
+      (6,0)   -0.10   ; RL_hip_joint
+      (7,0)   0.72   ; RL_thigh_joint
+      (8,0)  -1.44   ; RL_calf_joint
+      (9,0)   0.10   ; RR_hip_joint
+      (10,0)  0.72   ; RR_thigh_joint
+      (11,0) -1.44   ; RR_calf_joint
+}
+
+initialModeSchedule
+{
+  modeSequence
+  {
+    [0]  STANCE
+    [1]  STANCE
+  }
+  eventTimes
+  {
+    [0]  0.5
+  }
+}
+
+defaultModeSequenceTemplate
+{
+  modeSequence
+  {
+    [0]  STANCE
+  }
+  switchingTimes
+  {
+    [0]  0.0
+    [1]  1.0
+  }
+}
diff --git a/descriptions/xiaomi/cyberdog_description/config/ocs2/task.info b/descriptions/xiaomi/cyberdog_description/config/ocs2/task.info
new file mode 100644
index 0000000..d18089c
--- /dev/null
+++ b/descriptions/xiaomi/cyberdog_description/config/ocs2/task.info
@@ -0,0 +1,319 @@
+centroidalModelType             0      // 0: FullCentroidalDynamics, 1: Single Rigid Body Dynamics
+
+legged_robot_interface
+{
+  verbose                               false  // show the loaded parameters
+}
+
+model_settings
+{
+  positionErrorGain             0.0
+  phaseTransitionStanceTime     0.1
+
+  verboseCppAd                  true
+  recompileLibrariesCppAd       false
+  modelFolderCppAd              /tmp/ocs2_quadruped_controller/cyberdog
+}
+
+swing_trajectory_config
+{
+  liftOffVelocity               0.05
+  touchDownVelocity            -0.1
+  swingHeight                   0.1
+  swingTimeScale                0.15
+}
+
+; DDP settings
+ddp
+{
+  algorithm                       SLQ
+
+  nThreads                        3
+  threadPriority                  50
+
+  maxNumIterations                1
+  minRelCost                      1e-1
+  constraintTolerance             5e-3
+
+  displayInfo                     false
+  displayShortSummary             false
+  checkNumericalStability         false
+  debugPrintRollout               false
+  debugCaching                    false
+
+  AbsTolODE                       1e-5
+  RelTolODE                       1e-3
+  maxNumStepsPerSecond            10000
+  timeStep                        0.015
+  backwardPassIntegratorType      ODE45
+
+  constraintPenaltyInitialValue   20.0
+  constraintPenaltyIncreaseRate   2.0
+
+  preComputeRiccatiTerms          true
+
+  useFeedbackPolicy               false
+
+  strategy                        LINE_SEARCH
+  lineSearch
+  {
+    minStepLength                 1e-2
+    maxStepLength                 1.0
+    hessianCorrectionStrategy     DIAGONAL_SHIFT
+    hessianCorrectionMultiple     1e-5
+  }
+}
+
+
+; Multiple_Shooting SQP settings
+sqp
+{
+  nThreads                              3
+  dt                                    0.015
+  sqpIteration                          1
+  deltaTol                              1e-4
+  g_max                                 1e-2
+  g_min                                 1e-6
+  inequalityConstraintMu                0.1
+  inequalityConstraintDelta             5.0
+  projectStateInputEqualityConstraints  true
+  printSolverStatistics                 true
+  printSolverStatus                     false
+  printLinesearch                       false
+  useFeedbackPolicy                     false
+  integratorType                        RK2
+  threadPriority                        50
+}
+
+; Multiple_Shooting IPM settings
+ipm
+{
+  nThreads                              3
+  dt                                    0.015
+  ipmIteration                          1
+  deltaTol                              1e-4
+  g_max                                 10.0
+  g_min                                 1e-6
+  computeLagrangeMultipliers            true
+  printSolverStatistics                 true
+  printSolverStatus                     false
+  printLinesearch                       false
+  useFeedbackPolicy                     false
+  integratorType                        RK2
+  threadPriority                        50
+
+  initialBarrierParameter               1e-4
+  targetBarrierParameter                1e-4
+  barrierLinearDecreaseFactor           0.2
+  barrierSuperlinearDecreasePower       1.5
+  barrierReductionCostTol               1e-3
+  barrierReductionConstraintTol         1e-3
+
+  fractionToBoundaryMargin              0.995
+  usePrimalStepSizeForDual              false
+
+  initialSlackLowerBound                1e-4
+  initialDualLowerBound                 1e-4
+  initialSlackMarginRate                1e-2
+  initialDualMarginRate                 1e-2
+}
+
+; Rollout settings
+rollout
+{
+  AbsTolODE                       1e-5
+  RelTolODE                       1e-3
+  timeStep                        0.015
+  integratorType                  ODE45
+  maxNumStepsPerSecond            10000
+  checkNumericalStability         false
+}
+
+mpc
+{
+  timeHorizon                     1.0  ; [s]
+  solutionTimeWindow              -1   ; maximum [s]
+  coldStart                       false
+
+  debugPrint                      false
+
+  mpcDesiredFrequency             100  ; [Hz]
+  mrtDesiredFrequency             1000 ; [Hz] Useless
+}
+
+initialState
+{
+   ;; Normalized Centroidal Momentum: [linear, angular] ;;
+   (0,0)  0.0     ; vcom_x
+   (1,0)  0.0     ; vcom_y
+   (2,0)  0.0     ; vcom_z
+   (3,0)  0.0     ; L_x / robotMass
+   (4,0)  0.0     ; L_y / robotMass
+   (5,0)  0.0     ; L_z / robotMass
+
+   ;; Base Pose: [position, orientation] ;;
+   (6,0)  0.0     ; p_base_x
+   (7,0)  0.0     ; p_base_y
+   (8,0)  0.3     ; p_base_z
+   (9,0)  0.0     ; theta_base_z
+   (10,0) 0.0     ; theta_base_y
+   (11,0) 0.0     ; theta_base_x
+
+   ;; Leg Joint Positions: [FL, RL, FR, RR] ;;
+   (12,0) -0.20   ; FL_hip_joint
+   (13,0)  0.72   ; FL_thigh_joint
+   (14,0) -1.44   ; FL_calf_joint
+   (15,0) -0.20   ; RL_hip_joint
+   (16,0)  0.72   ; RL_thigh_joint
+   (17,0) -1.44   ; RL_calf_joint
+   (18,0)  0.20   ; FR_hip_joint
+   (19,0)  0.72   ; FR_thigh_joint
+   (20,0) -1.44   ; FR_calf_joint
+   (21,0)  0.20   ; RR_hip_joint
+   (22,0)  0.72   ; RR_thigh_joint
+   (23,0) -1.44   ; RR_calf_joint
+}
+
+; standard state weight matrix
+Q
+{
+  scaling 1e+0
+
+  ;; Normalized Centroidal Momentum: [linear, angular] ;;
+  (0,0)   15.0     ; vcom_x
+  (1,1)   15.0     ; vcom_y
+  (2,2)   100.0    ; vcom_z
+  (3,3)   10.0     ; L_x / robotMass
+  (4,4)   30.0     ; L_y / robotMass
+  (5,5)   30.0     ; L_z / robotMass
+
+  ;; Base Pose: [position, orientation] ;;
+  (6,6)   1000.0   ; p_base_x
+  (7,7)   1000.0   ; p_base_y
+  (8,8)   1500.0   ; p_base_z
+  (9,9)   100.0    ; theta_base_z
+  (10,10) 300.0    ; theta_base_y
+  (11,11) 300.0    ; theta_base_x
+
+  ;; Leg Joint Positions: [FL, RL, FR, RR] ;;
+  (12,12) 5.0     ; FL_hip_joint
+  (13,13) 5.0     ; FL_thigh_joint
+  (14,14) 2.5     ; FL_calf_joint
+  (15,15) 5.0     ; RL_hip_joint
+  (16,16) 5.0     ; RL_thigh_joint
+  (17,17) 2.5     ; RL_calf_joint
+  (18,18) 5.0     ; FR_hip_joint
+  (19,19) 5.0     ; FR_thigh_joint
+  (20,20) 2.5     ; FR_calf_joint
+  (21,21) 5.0     ; RR_hip_joint
+  (22,22) 5.0     ; RR_thigh_joint
+  (23,23) 2.5     ; RR_calf_joint
+}
+
+; control weight matrix
+R
+{
+  scaling 1e-3
+
+  ;; Feet Contact Forces: [FL, FR, RL, RR] ;;
+  (0,0)   1.0       ; front_left_force
+  (1,1)   1.0       ; front_left_force
+  (2,2)   1.0       ; front_left_force
+  (3,3)   1.0       ; front_right_force
+  (4,4)   1.0       ; front_right_force
+  (5,5)   1.0       ; front_right_force
+  (6,6)   1.0       ; rear_left_force
+  (7,7)   1.0       ; rear_left_force
+  (8,8)   1.0       ; rear_left_force
+  (9,9)   1.0       ; rear_right_force
+  (10,10) 1.0       ; rear_right_force
+  (11,11) 1.0       ; rear_right_force
+
+  ;; foot velocity relative to base: [FL, RL, FR, RR] (uses the Jacobian at nominal configuration) ;;
+  (12,12) 5000.0    ; x
+  (13,13) 5000.0    ; y
+  (14,14) 5000.0    ; z
+  (15,15) 5000.0    ; x
+  (16,16) 5000.0    ; y
+  (17,17) 5000.0    ; z
+  (18,18) 5000.0    ; x
+  (19,19) 5000.0    ; y
+  (20,20) 5000.0    ; z
+  (21,21) 5000.0    ; x
+  (22,22) 5000.0    ; y
+  (23,23) 5000.0    ; z
+}
+
+frictionConeSoftConstraint
+{
+  frictionCoefficient    0.3
+
+  ; relaxed log barrier parameters
+  mu                     0.1
+  delta                  5.0
+}
+
+selfCollision
+{
+  ; Self Collision raw object pairs
+  collisionObjectPairs
+  {
+  }
+
+  ; Self Collision pairs
+  collisionLinkPairs
+  {
+    [0] "FL_calf, FR_calf"
+    [1] "RL_calf, RR_calf"
+    [2] "FL_calf, RL_calf"
+    [3] "FR_calf, RR_calf"
+    [4] "FL_foot, FR_foot"
+    [5] "RL_foot, RR_foot"
+    [6] "FL_foot, RL_foot"
+    [7] "FR_foot, RR_foot"
+  }
+
+  minimumDistance  0.05
+
+  ; relaxed log barrier parameters
+  mu      1e-2
+  delta   1e-3
+}
+
+; Whole body control
+torqueLimitsTask
+{
+   (0,0)  33.5     ; HAA
+   (1,0)  33.5     ; HFE
+   (2,0)  33.5     ; KFE
+}
+
+frictionConeTask
+{
+  frictionCoefficient    0.3
+}
+
+swingLegTask
+{
+    kp                   350
+    kd                   37
+}
+
+weight
+{
+    swingLeg        100
+    baseAccel       1
+    contactForce    0.01
+}
+
+; State Estimation
+kalmanFilter
+{
+    footRadius                  0.02
+    imuProcessNoisePosition     0.02
+    imuProcessNoiseVelocity     0.02
+    footProcessNoisePosition    0.002
+    footSensorNoisePosition     0.005
+    footSensorNoiseVelocity     0.1
+    footHeightSensorNoise       0.01
+}
diff --git a/descriptions/xiaomi/cyberdog_description/config/robot_control.yaml b/descriptions/xiaomi/cyberdog_description/config/robot_control.yaml
new file mode 100644
index 0000000..ab6782a
--- /dev/null
+++ b/descriptions/xiaomi/cyberdog_description/config/robot_control.yaml
@@ -0,0 +1,159 @@
+# Controller Manager configuration
+controller_manager:
+  ros__parameters:
+    update_rate: 500  # 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
+
+    ocs2_quadruped_controller:
+      type: ocs2_quadruped_controller/Ocs2QuadrupedController
+
+imu_sensor_broadcaster:
+  ros__parameters:
+    sensor_name: "imu_sensor"
+    frame_id: "imu_link"
+
+#leg_pd_controller:
+#  ros__parameters:
+#    joints:
+#      - FR_abad_joint
+#      - FR_hip_joint
+#      - FR_knee_joint
+#      - FL_abad_joint
+#      - FL_hip_joint
+#      - FL_knee_joint
+#      - RR_abad_joint
+#      - RR_hip_joint
+#      - RR_knee_joint
+#      - RL_abad_joint
+#      - RL_hip_joint
+#      - RL_knee_joint
+#
+#    command_interfaces:
+#      - effort
+#
+#    state_interfaces:
+#      - position
+#      - velocity
+#
+#unitree_guide_controller:
+#  ros__parameters:
+#    command_prefix: "leg_pd_controller"
+#    joints:
+#      - FR_abad_joint
+#      - FR_hip_joint
+#      - FR_knee_joint
+#      - FL_abad_joint
+#      - FL_hip_joint
+#      - FL_knee_joint
+#      - RR_abad_joint
+#      - RR_hip_joint
+#      - RR_knee_joint
+#      - RL_abad_joint
+#      - RL_hip_joint
+#      - RL_knee_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_link"
+#
+#    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
+
+
+ocs2_quadruped_controller:
+  ros__parameters:
+    update_rate: 500  # Hz
+    default_kd: 1.0
+
+    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
+
+    feet_names:
+      - FL_foot
+      - FR_foot
+      - RL_foot
+      - RR_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
+
+    foot_force_name: "foot_force"
+    foot_force_interfaces:
+      - FL
+      - RL
+      - FR
+      - RR
\ No newline at end of file
diff --git a/descriptions/cyberdog_description/launch/gazebo.launch.py b/descriptions/xiaomi/cyberdog_description/launch/gazebo.launch.py
similarity index 100%
rename from descriptions/cyberdog_description/launch/gazebo.launch.py
rename to descriptions/xiaomi/cyberdog_description/launch/gazebo.launch.py
diff --git a/descriptions/xiaomi/cyberdog_description/launch/ocs2_control.launch.py b/descriptions/xiaomi/cyberdog_description/launch/ocs2_control.launch.py
new file mode 100644
index 0000000..b35e736
--- /dev/null
+++ b/descriptions/xiaomi/cyberdog_description/launch/ocs2_control.launch.py
@@ -0,0 +1,122 @@
+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.event_handlers import OnProcessExit
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from sympy.physics.vector.printing import params
+
+package_description = "cyberdog_description"
+package_controller = "ocs2_quadruped_controller"
+
+def process_xacro(context):
+    robot_type_value = context.launch_configurations['robot_type']
+    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={'robot_type': robot_type_value})
+    return (robot_description_config.toxml(), robot_type_value)
+
+
+def launch_setup(context, *args, **kwargs):
+    (robot_description, robot_type) = process_xacro(context)
+    robot_controllers = PathJoinSubstitution(
+        [
+            FindPackageShare(package_description),
+            "config",
+            "robot_control.yaml",
+        ]
+    )
+
+    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
+            }
+        ],
+    )
+
+    controller_manager = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[robot_controllers,
+                    {
+                        'urdf_file': os.path.join(get_package_share_directory(package_description), 'urdf', 'robot.urdf'),
+                        'task_file': os.path.join(get_package_share_directory(package_description), 'config', 'ocs2',
+                                                  'task.info'),
+                        'reference_file': os.path.join(get_package_share_directory(package_description), 'config',
+                                                       'ocs2', 'reference.info'),
+                        'gait_file': os.path.join(get_package_share_directory(package_description), 'config',
+                                                       'ocs2', 'gait.info')
+                    }],
+        output="both",
+    )
+
+    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"],
+    )
+
+    ocs2_controller = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["ocs2_quadruped_controller", "--controller-manager", "/controller_manager"]
+    )
+
+    return [
+        robot_state_publisher,
+        controller_manager,
+        joint_state_publisher,
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=joint_state_publisher,
+                on_exit=[imu_sensor_broadcaster],
+            )
+        ),
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=imu_sensor_broadcaster,
+                on_exit=[ocs2_controller],
+            )
+        ),
+    ]
+
+
+def generate_launch_description():
+    robot_type_arg = DeclareLaunchArgument(
+        'robot_type',
+        default_value='go1',
+        description='Type of the robot'
+    )
+
+    rviz_config_file = os.path.join(get_package_share_directory(package_controller), "config", "visualize_ocs2.rviz")
+
+    return LaunchDescription([
+        robot_type_arg,
+        OpaqueFunction(function=launch_setup),
+        Node(
+            package='rviz2',
+            executable='rviz2',
+            name='rviz_ocs2',
+            output='screen',
+            arguments=["-d", rviz_config_file]
+        )
+    ])
diff --git a/descriptions/cyberdog_description/launch/visualize.launch.py b/descriptions/xiaomi/cyberdog_description/launch/visualize.launch.py
similarity index 100%
rename from descriptions/cyberdog_description/launch/visualize.launch.py
rename to descriptions/xiaomi/cyberdog_description/launch/visualize.launch.py
diff --git a/descriptions/cyberdog_description/meshes/abad.dae b/descriptions/xiaomi/cyberdog_description/meshes/abad.dae
similarity index 100%
rename from descriptions/cyberdog_description/meshes/abad.dae
rename to descriptions/xiaomi/cyberdog_description/meshes/abad.dae
diff --git a/descriptions/cyberdog_description/meshes/body.dae b/descriptions/xiaomi/cyberdog_description/meshes/body.dae
similarity index 100%
rename from descriptions/cyberdog_description/meshes/body.dae
rename to descriptions/xiaomi/cyberdog_description/meshes/body.dae
diff --git a/descriptions/cyberdog_description/meshes/hip.dae b/descriptions/xiaomi/cyberdog_description/meshes/hip.dae
similarity index 100%
rename from descriptions/cyberdog_description/meshes/hip.dae
rename to descriptions/xiaomi/cyberdog_description/meshes/hip.dae
diff --git a/descriptions/cyberdog_description/meshes/hip_mirror.dae b/descriptions/xiaomi/cyberdog_description/meshes/hip_mirror.dae
similarity index 100%
rename from descriptions/cyberdog_description/meshes/hip_mirror.dae
rename to descriptions/xiaomi/cyberdog_description/meshes/hip_mirror.dae
diff --git a/descriptions/cyberdog_description/meshes/knee.dae b/descriptions/xiaomi/cyberdog_description/meshes/knee.dae
similarity index 100%
rename from descriptions/cyberdog_description/meshes/knee.dae
rename to descriptions/xiaomi/cyberdog_description/meshes/knee.dae
diff --git a/descriptions/cyberdog_description/package.xml b/descriptions/xiaomi/cyberdog_description/package.xml
similarity index 100%
rename from descriptions/cyberdog_description/package.xml
rename to descriptions/xiaomi/cyberdog_description/package.xml
diff --git a/descriptions/xiaomi/cyberdog_description/xacro/const.xacro b/descriptions/xiaomi/cyberdog_description/xacro/const.xacro
new file mode 100644
index 0000000..54eca40
--- /dev/null
+++ b/descriptions/xiaomi/cyberdog_description/xacro/const.xacro
@@ -0,0 +1,125 @@
+<?xml version="1.0"?>
+
+<robot name="cyberdog" xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:property name="PI" value="3.1415926535897931"/>
+
+  <!-- dimension -->
+  <!-- body -->
+  <xacro:property name="body_width" value="0.190"/>
+  <xacro:property name="body_length" value="0.236"/>
+  <xacro:property name="body_height" value="0.109"/>
+
+  <!-- abad -->
+  <xacro:property name="abad_radius" value="0.039"/>
+  <xacro:property name="abad_length" value="0.035"/>
+  <xacro:property name="abad_offset_x" value="0.164"/>
+  <xacro:property name="abad_offset_y" value="0.042"/>
+  <xacro:property name="abad_rotor_offset" value="0.0642303"/>
+  <xacro:property name="abad_max" value="39"/>
+  <xacro:property name="abad_min" value="-39"/>
+
+  <!-- hip -->
+  <xacro:property name="hip_shoulder_radius" value="0.039"/>
+  <xacro:property name="hip_shoulder_length" value="0.035"/>
+  <xacro:property name="hip_width" value="0.0250"/>
+  <xacro:property name="hip_height" value="0.034"/>
+  <xacro:property name="hip_offset" value="0.094"/>
+  <xacro:property name="hip_length" value="0.12"/>
+  <xacro:property name="hip_rotor_offset" value="-0.07577"/>
+  <xacro:property name="hip_f_max" value="160"/>
+  <xacro:property name="hip_f_min" value="-76"/>
+  <xacro:property name="hip_h_max" value="180"/>
+  <xacro:property name="hip_h_min" value="-56"/>
+
+  <!-- knee -->
+  <xacro:property name="knee_width" value="0.016"/>
+  <xacro:property name="knee_height" value="0.016"/>
+  <xacro:property name="knee_length" value="0.18341"/>
+  <xacro:property name="knee_rotor_offset" value="-0.0342303"/>
+  <xacro:property name="knee_max" value="-30"/>
+  <xacro:property name="knee_min" value="-145"/>
+  <xacro:property name="knee_rubber" value="0.02"/>
+
+  <!-- foot -->
+  <xacro:property name="foot_radius" value="0.019"/>
+
+  <!-- acuator atribute -->
+  <xacro:property name="abadGearRatio" value="7.75"/>
+  <xacro:property name="hipGearRatio" value="7.75"/>
+  <xacro:property name="kneeGearRatio" value="7.75"/>
+
+  <xacro:property name="abad_motorTauMax" value="1.5484"/>
+  <xacro:property name="hip_motorTauMax" value="1.5484"/>
+  <xacro:property name="knee_motorTauMax" value="1.5484"/>
+  <xacro:property name="abad_motorVelMax" value="30.9971"/>
+  <xacro:property name="hip_motorVelMax" value="30.9971"/>
+  <xacro:property name="knee_motorVelMax" value="30.9971"/>
+  <xacro:property name="abad_damping" value="0.01"/>
+  <xacro:property name="hip_damping" value="0.01"/>
+  <xacro:property name="knee_damping" value="0.01"/>
+  <xacro:property name="abad_friction" value="0.1"/>
+  <xacro:property name="hip_friction" value="0.1"/>
+  <xacro:property name="knee_friction" value="0.1"/>
+
+  <!-- inertial&mass value -->
+  <!-- body -->
+  <xacro:property name="body_mass" value="4.030000000"/>
+  <xacro:property name="body_com_x" value="0.027300000"/>
+  <xacro:property name="body_com_y" value="-0.000242000"/>
+  <xacro:property name="body_com_z" value="0.014300000"/>
+  <xacro:property name="body_ixx" value="0.018500000"/>
+  <xacro:property name="body_ixy" value="-0.000173000"/>
+  <xacro:property name="body_ixz" value="-0.010200000"/>
+  <xacro:property name="body_iyy" value="0.051700000"/>
+  <xacro:property name="body_iyz" value="-0.000028300"/>
+  <xacro:property name="body_izz" value="0.048300000"/>
+
+  <!-- abad -->
+  <xacro:property name="abad_mass" value="0.354000000"/>
+  <xacro:property name="abad_com_x" value="-0.003920000"/>
+  <xacro:property name="abad_com_y" value="0.015000000"/>
+  <xacro:property name="abad_com_z" value="-0.000306000"/>
+  <xacro:property name="abad_ixx" value="0.000190000"/>
+  <xacro:property name="abad_ixy" value="-0.000027000"/>
+  <xacro:property name="abad_ixz" value="-0.000000344"/>
+  <xacro:property name="abad_iyy" value="0.000276000"/>
+  <xacro:property name="abad_iyz" value="0.000001950"/>
+  <xacro:property name="abad_izz" value="0.000233000"/>
+
+  <!-- hip -->
+  <xacro:property name="hip_mass" value="0.482000000"/>
+  <xacro:property name="hip_com_x" value="-0.002120000"/>
+  <xacro:property name="hip_com_y" value="-0.021200000"/>
+  <xacro:property name="hip_com_z" value="-0.018400000"/>
+  <xacro:property name="hip_ixx" value="0.001010000"/>
+  <xacro:property name="hip_ixy" value="0.000022300"/>
+  <xacro:property name="hip_ixz" value="-0.000038500"/>
+  <xacro:property name="hip_iyy" value="0.000983000"/>
+  <xacro:property name="hip_iyz" value="0.000199000"/>
+  <xacro:property name="hip_izz" value="0.000347000"/>
+
+  <!-- knee -->
+  <xacro:property name="knee_mass" value="0.116000000"/>
+  <xacro:property name="knee_com_x" value="0.000600000"/>
+  <xacro:property name="knee_com_y" value="-0.000047200"/>
+  <xacro:property name="knee_com_z" value="-0.089300000"/>
+  <xacro:property name="knee_ixx" value="0.000668000"/>
+  <xacro:property name="knee_ixy" value="0.000000003"/>
+  <xacro:property name="knee_ixz" value="0.000023700"/>
+  <xacro:property name="knee_iyy" value="0.000674000"/>
+  <xacro:property name="knee_iyz" value="0.000000603"/>
+  <xacro:property name="knee_izz" value="0.000015400"/>
+
+  <!-- rotor -->
+  <xacro:property name="rotor_mass" value="0.056700000"/>
+  <xacro:property name="rotor_com_x" value="0"/>
+  <xacro:property name="rotor_com_y" value="0"/>
+  <xacro:property name="rotor_com_z" value="0"/>
+  <xacro:property name="rotor_ixx" value="0.000025300"/>
+  <xacro:property name="rotor_ixy" value="0"/>
+  <xacro:property name="rotor_ixz" value="0"/>
+  <xacro:property name="rotor_iyy" value="0.000047800"/>
+  <xacro:property name="rotor_iyz" value="0"/>
+  <xacro:property name="rotor_izz" value="0.000025300"/>
+</robot>
\ No newline at end of file
diff --git a/descriptions/xiaomi/cyberdog_description/xacro/gazebo.xacro b/descriptions/xiaomi/cyberdog_description/xacro/gazebo.xacro
new file mode 100644
index 0000000..84d65e8
--- /dev/null
+++ b/descriptions/xiaomi/cyberdog_description/xacro/gazebo.xacro
@@ -0,0 +1,214 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <ros2_control name="GazeboSystem" type="system">
+    <hardware>
+      <plugin>gz_ros2_control/GazeboSimSystem</plugin>
+    </hardware>
+
+    <joint name="FR_abad_joint">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="FR_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="FR_knee_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_abad_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_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_knee_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_abad_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_knee_joint">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="RL_abad_joint">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="RL_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="RL_knee_joint">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <sensor name="imu_sensor">
+      <state_interface name="orientation.x"/>
+      <state_interface name="orientation.y"/>
+      <state_interface name="orientation.z"/>
+      <state_interface name="orientation.w"/>
+      <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>
+
+    <sensor name="LF_foot_end">
+      <state_interface name="force.x"/>
+      <state_interface name="force.y"/>
+      <state_interface name="force.z"/>
+      <param name="frame_id">LF_FOOT</param>
+    </sensor>
+    <sensor name="RF_foot_end">
+      <state_interface name="force.x"/>
+      <state_interface name="force.y"/>
+      <state_interface name="force.z"/>
+      <param name="frame_id">RF_FOOT</param>
+    </sensor>
+    <sensor name="LH_foot_end">
+      <state_interface name="force.x"/>
+      <state_interface name="force.y"/>
+      <state_interface name="force.z"/>
+      <param name="frame_id">LH_FOOT</param>
+    </sensor>
+    <sensor name="RH_foot_end">
+      <state_interface name="force.x"/>
+      <state_interface name="force.y"/>
+      <state_interface name="force.z"/>
+      <param name="frame_id">LH_FOOT</param>
+    </sensor>
+  </ros2_control>
+
+  <!-- Gazebo's ros2_control plugin  -->
+  <gazebo>
+    <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
+      <parameters>$(find cyberdog_description)/config/robot_control.yaml</parameters>
+    </plugin>
+    <plugin filename="gz-sim-imu-system"
+            name="gz::sim::systems::Imu">
+    </plugin>
+    <plugin filename="gz-sim-contact-system"
+            name="gz::sim::systems::Contact">
+    </plugin>
+    <plugin filename="gz-sim-forcetorque-system"
+            name="gz::sim::systems::ForceTorque">
+    </plugin>
+  </gazebo>
+
+  <gazebo reference="imu_link">
+    <sensor name="imu_sensor" type="imu">
+      <always_on>1</always_on>
+      <update_rate>500</update_rate>
+      <visualize>true</visualize>
+      <topic>imu</topic>
+      <imu>
+        <angular_velocity>
+          <x>
+            <noise type="gaussian">
+              <mean>0.0</mean>
+              <stddev>2e-4</stddev>
+              <bias_mean>0.0000075</bias_mean>
+              <bias_stddev>0.0000008</bias_stddev>
+            </noise>
+          </x>
+          <y>
+            <noise type="gaussian">
+              <mean>0.0</mean>
+              <stddev>2e-4</stddev>
+              <bias_mean>0.0000075</bias_mean>
+              <bias_stddev>0.0000008</bias_stddev>
+            </noise>
+          </y>
+          <z>
+            <noise type="gaussian">
+              <mean>0.0</mean>
+              <stddev>2e-4</stddev>
+              <bias_mean>0.0000075</bias_mean>
+              <bias_stddev>0.0000008</bias_stddev>
+            </noise>
+          </z>
+        </angular_velocity>
+        <linear_acceleration>
+          <x>
+            <noise type="gaussian">
+              <mean>0.0</mean>
+              <stddev>1.7e-2</stddev>
+              <bias_mean>0.1</bias_mean>
+              <bias_stddev>0.001</bias_stddev>
+            </noise>
+          </x>
+          <y>
+            <noise type="gaussian">
+              <mean>0.0</mean>
+              <stddev>1.7e-2</stddev>
+              <bias_mean>0.1</bias_mean>
+              <bias_stddev>0.001</bias_stddev>
+            </noise>
+          </y>
+          <z>
+            <noise type="gaussian">
+              <mean>0.0</mean>
+              <stddev>1.7e-2</stddev>
+              <bias_mean>0.1</bias_mean>
+              <bias_stddev>0.001</bias_stddev>
+            </noise>
+          </z>
+        </linear_acceleration>
+      </imu>
+    </sensor>
+  </gazebo>
+  <gazebo reference="imu_joint">
+    <disableFixedJointLumping>true</disableFixedJointLumping>
+  </gazebo>
+
+</robot>
\ No newline at end of file
diff --git a/descriptions/xiaomi/cyberdog_description/xacro/leg.xacro b/descriptions/xiaomi/cyberdog_description/xacro/leg.xacro
new file mode 100644
index 0000000..be3b49d
--- /dev/null
+++ b/descriptions/xiaomi/cyberdog_description/xacro/leg.xacro
@@ -0,0 +1,295 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:include filename="$(find cyberdog_description)/xacro/transmission.xacro"/>
+
+  <xacro:macro name="leg" params="name mirror mirror_dae front_hind front_hind_dae *origin">
+
+    <joint name="${name}_hip_joint" type="revolute">
+      <xacro:insert_block name="origin"/>
+      <parent link="body"/>
+      <child link="${name}_abad"/>
+      <axis xyz="1 0 0"/>
+      <dynamics damping="${abad_damping}" friction="${abad_friction}"/>
+      <xacro:if value="${(mirror_dae == True)}">
+        <limit effort="${abad_motorTauMax*abadGearRatio}"
+               velocity="${abad_motorVelMax}" lower="${abad_min*PI/180.0}"
+               upper="${abad_max*PI/180.0}"/>
+      </xacro:if>
+      <xacro:if value="${(mirror_dae == False)}">
+        <limit effort="${abad_motorTauMax*abadGearRatio}"
+               velocity="${abad_motorVelMax}" lower="${-abad_max*PI/180.0}"
+               upper="${-abad_min*PI/180.0}"/>
+      </xacro:if>
+    </joint>
+
+    <link name="${name}_abad">
+      <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="file://$(find cyberdog_description)/meshes/abad.dae" scale="1 1 1"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
+        <geometry>
+          <cylinder length="${abad_length}" radius="${abad_radius}"/>
+        </geometry>
+      </collision>
+      <inertial>
+        <origin rpy="0 0 0"
+                xyz="${abad_com_x*front_hind} ${abad_com_y*mirror} ${abad_com_z}"/>
+        <mass value="${abad_mass}"/>
+        <inertia ixx="${abad_ixx}" ixy="${abad_ixy*mirror*front_hind}"
+                 ixz="${abad_ixz*front_hind}" iyy="${abad_iyy}" iyz="${abad_iyz*mirror}"
+                 izz="${abad_izz}"/>
+      </inertial>
+    </link>
+
+    <joint name="${name}_abad_rotor_fix" type="fixed">
+      <origin rpy="0 0 0" xyz="${-abad_rotor_offset*front_hind} 0 0"/>
+      <parent link="${name}_abad"/>
+      <child link="${name}_abad_rotor"/>
+    </joint>
+
+    <!-- this link is only for abad rotor inertial -->
+    <link name="${name}_abad_rotor">
+      <inertial>
+        <origin rpy="0 0 0" xyz="${rotor_com_x} ${rotor_com_y} ${rotor_com_z}"/>
+        <mass value="${rotor_mass}"/>
+        <inertia ixx="${rotor_ixx}" ixy="${rotor_ixy}" ixz="${rotor_ixz}" iyy="${rotor_iyy}" iyz="${rotor_iyz}"
+                 izz="${rotor_izz}"/>
+      </inertial>
+    </link>
+
+    <joint name="${name}_abad_fixed" type="fixed">
+      <origin rpy="0 0 0" xyz="0 ${(hip_offset-hip_shoulder_length)*mirror} 0"/>
+      <parent link="${name}_abad"/>
+      <child link="${name}_hip_shoulder"/>
+    </joint>
+
+    <link name="${name}_hip_shoulder">
+      <collision>
+        <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
+        <geometry>
+          <cylinder length="${hip_shoulder_length}" radius="${hip_shoulder_radius}"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="${name}_thigh_joint" type="revolute">
+      <origin rpy="0 0 0" xyz="0 ${hip_offset*mirror} 0"/>
+      <parent link="${name}_abad"/>
+      <child link="${name}_hip"/>
+      <axis xyz="0 1 0"/>
+      <dynamics damping="${hip_damping}" friction="${hip_friction}"/>
+      <xacro:if value="${front_hind_dae == True}">
+        <limit effort="${hip_motorTauMax*hipGearRatio}"
+               velocity="${hip_motorVelMax}" lower="${hip_f_min*PI/180.0}"
+               upper="${hip_f_max*PI/180.0}"/>
+      </xacro:if>
+      <xacro:if value="${front_hind_dae == False}">
+        <limit effort="${hip_motorTauMax*hipGearRatio}"
+               velocity="${hip_motorVelMax}" lower="${hip_h_min*PI/180.0}"
+               upper="${hip_h_max*PI/180.0}"/>
+      </xacro:if>
+    </joint>
+
+    <link name="${name}_hip">
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <xacro:if value="${mirror_dae == True}">
+            <mesh filename="file://$(find cyberdog_description)/meshes/hip.dae"
+                  scale="1 1 1"/>
+          </xacro:if>
+          <xacro:if value="${mirror_dae == False}">
+            <mesh filename="file://$(find cyberdog_description)/meshes/hip_mirror.dae"
+                  scale="1 1 1"/>
+          </xacro:if>
+        </geometry>
+      </visual>
+      <collision>
+        <origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-hip_length/2.0}"/>
+        <geometry>
+          <box size="${hip_length} ${hip_width} ${hip_height}"/>
+        </geometry>
+      </collision>
+      <inertial>
+        <origin rpy="0 0 0" xyz="${hip_com_x} ${hip_com_y*mirror} ${hip_com_z}"/>
+        <mass value="${hip_mass}"/>
+        <inertia ixx="${hip_ixx}" ixy="${hip_ixy*mirror}" ixz="${hip_ixz}" iyy="${hip_iyy}"
+                 iyz="${hip_iyz*mirror}" izz="${hip_izz}"/>
+      </inertial>
+    </link>
+
+    <joint name="${name}_hip_rotor_fix" type="fixed">
+      <origin rpy="0 0 0" xyz="0 ${hip_rotor_offset*mirror} 0"/>
+      <parent link="${name}_hip"/>
+      <child link="${name}_hip_rotor"/>
+    </joint>
+
+    <!-- this link is only for hip rotor inertial -->
+    <link name="${name}_hip_rotor">
+      <inertial>
+        <origin rpy="0 0 0" xyz="${rotor_com_x} ${rotor_com_y} ${rotor_com_z}"/>
+        <mass value="${rotor_mass}"/>
+        <inertia ixx="${rotor_ixx}" ixy="${rotor_ixy}" ixz="${rotor_ixz}" iyy="${rotor_iyy}" iyz="${rotor_iyz}"
+                 izz="${rotor_izz}"/>
+      </inertial>
+    </link>
+
+    <joint name="${name}_knee_rotor_fix" type="fixed">
+      <origin rpy="0 0 0" xyz="0 ${knee_rotor_offset*mirror} 0"/>
+      <parent link="${name}_hip"/>
+      <child link="${name}_knee_rotor"/>
+    </joint>
+
+    <!-- this link is only for knee rotor inertial -->
+    <link name="${name}_knee_rotor">
+      <inertial>
+        <origin rpy="0 0 0" xyz="${rotor_com_x} ${rotor_com_y} ${rotor_com_z}"/>
+        <mass value="${rotor_mass}"/>
+        <inertia ixx="${rotor_ixx}" ixy="${rotor_ixy}" ixz="${rotor_ixz}" iyy="${rotor_iyy}" iyz="${rotor_iyz}"
+                 izz="${rotor_izz}"/>
+      </inertial>
+    </link>
+
+    <joint name="${name}_calf_joint" type="revolute">
+      <origin rpy="0 0 0" xyz="0 0 ${-hip_length}"/>
+      <parent link="${name}_hip"/>
+      <child link="${name}_knee"/>
+      <axis xyz="0 1 0"/>
+      <dynamics damping="${hip_damping}" friction="${hip_friction}"/>
+      <limit effort="${knee_motorTauMax*kneeGearRatio}"
+             velocity="${knee_motorVelMax}" lower="${knee_min*PI/180.0}"
+             upper="${knee_max*PI/180.0}"/>
+    </joint>
+
+    <link name="${name}_knee">
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://$(find cyberdog_description)/meshes/knee.dae" scale="1 1 1"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin rpy="0 ${PI} 0" xyz="0 0 ${-knee_length/2.0}"/>
+        <geometry>
+          <box size="${knee_height} ${knee_width} ${knee_length}"/>
+        </geometry>
+      </collision>
+      <collision name="${name}_knee_rubber">
+        <origin rpy="0 ${PI} 0" xyz="${-knee_rubber/2.0} 0 -0.007"/>
+        <geometry>
+          <sphere radius="0.016"/>
+        </geometry>
+        <surface>
+          <contact>
+            <ode>
+              <max_vel>0.00001</max_vel>
+              <min_depth>0.0</min_depth>
+            </ode>
+          </contact>
+        </surface>
+      </collision>
+      <inertial>
+        <origin rpy="0 0 0" xyz="${knee_com_x} ${knee_com_y} ${knee_com_z}"/>
+        <mass value="${knee_mass}"/>
+        <inertia ixx="${knee_ixx}" ixy="${knee_ixy}" ixz="${knee_ixz}" iyy="${knee_iyy}"
+                 iyz="${knee_iyz}" izz="${knee_izz}"/>
+      </inertial>
+    </link>
+
+    <joint name="${name}_foot_fixed" type="fixed">
+      <origin rpy="0 0 0" xyz="0.0055 0 ${-knee_length+foot_radius/2.0}"/>
+      <parent link="${name}_knee"/>
+      <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>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <sphere radius="${foot_radius}"/>
+        </geometry>
+        <surface>
+          <contact>
+            <ode>
+              <max_vel>0.001</max_vel>
+              <min_depth>0.0</min_depth>
+            </ode>
+          </contact>
+        </surface>
+      </collision>
+    </link>
+
+    <xacro:leg_transmission name="${name}"/>
+
+    <gazebo reference="${name}_abad">
+      <mu1>0.2</mu1>
+      <mu2>0.2</mu2>
+      <visual>
+        <material>
+          <ambient>.5 .5 .5 1.0</ambient>
+          <diffuse>.5 .5 .5 1.0</diffuse>
+          <specular>.5 .5 .5 1.0</specular>
+        </material>
+      </visual>
+    </gazebo>
+    <gazebo reference="${name}_hip">
+      <mu1>0.2</mu1>
+      <mu2>0.2</mu2>
+      <self_collide>1</self_collide>
+      <visual>
+        <material>
+          <ambient>.05 .05 .05 1.0</ambient>
+          <diffuse>.05 .05 .05 1.0</diffuse>
+          <specular>.05 .05 .05 1.0</specular>
+        </material>
+      </visual>
+    </gazebo>
+    <gazebo reference="${name}_knee">
+      <mu1>0.7</mu1>
+      <mu2>0.7</mu2>
+      <self_collide>1</self_collide>
+      <visual>
+        <material>
+          <ambient>.5 .5 .5 1.0</ambient>
+          <diffuse>.5 .5 .5 1.0</diffuse>
+          <specular>.5 .5 .5 1.0</specular>
+        </material>
+      </visual>
+    </gazebo>
+    <gazebo reference="${name}_foot">
+      <mu1>0.7</mu1>
+      <mu2>0.7</mu2>
+      <self_collide>1</self_collide>
+      <visual>
+        <material>
+          <ambient>.5 .5 .5 1.0</ambient>
+          <diffuse>.5 .5 .5 1.0</diffuse>
+          <specular>.5 .5 .5 1.0</specular>
+        </material>
+      </visual>
+    </gazebo>
+
+  </xacro:macro>
+</robot>
\ No newline at end of file
diff --git a/descriptions/xiaomi/cyberdog_description/xacro/robot.xacro b/descriptions/xiaomi/cyberdog_description/xacro/robot.xacro
new file mode 100644
index 0000000..ae91257
--- /dev/null
+++ b/descriptions/xiaomi/cyberdog_description/xacro/robot.xacro
@@ -0,0 +1,212 @@
+<?xml version="1.0"?>
+<robot name="cyber_dog" xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:arg name="ROBOT" default="cyber_dog"/>
+  <xacro:arg name="USE_LIDAR" default="false"/>
+  <xacro:include filename="const.xacro"/>
+  <xacro:include filename="leg.xacro"/>
+  <!--    <xacro:include filename="gazebo.xacro"/>-->
+  <xacro:include filename="ros2_control.xacro"/>
+
+  <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="body"/>
+  </joint>
+
+  <link name="body">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find cyberdog_description)/meshes/body.dae" scale="1 1 1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="${body_length} ${body_width} ${body_height}"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="${body_com_x} ${body_com_y} ${body_com_z}"/>
+      <mass value="${body_mass}"/>
+      <inertia ixx="${body_ixx}" ixy="${body_ixy}" ixz="${body_ixz}" iyy="${body_iyy}" iyz="${body_iyz}"
+               izz="${body_izz}"/>
+    </inertial>
+  </link>
+
+  <joint name="imu_joint" type="fixed">
+    <parent link="body"/>
+    <child link="imu_link"/>
+    <origin rpy="0 0 0" xyz="33.4e-3 -17.2765e-3 51.0469e-3"/>
+  </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>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size=".001 .001 .001"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="scan_joint" type="fixed">
+    <parent link="body"/>
+    <child link="lidar_link"/>
+    <origin rpy="0 0 0" xyz="0.21425 0 0.0908"/>
+  </joint>
+
+  <link name="lidar_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>
+  </link>
+
+  <joint name="D435_camera_joint" type="fixed">
+    <parent link="body"/>
+    <child link="D435_camera_link"/>
+    <origin rpy="0 0 0" xyz="271.994e-3 25e-3 114.912e-3"/>
+  </joint>
+
+  <link name="D435_camera_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>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size=".001 .001 .001"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="RGB_camera_joint" type="fixed">
+    <parent link="body"/>
+    <child link="RGB_camera_link"/>
+    <origin rpy="0 0 0" xyz="275.76e-3 0 125.794e-3"/>
+  </joint>
+
+  <link name="RGB_camera_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>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size=".001 .001 .001"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="AI_camera_joint" type="fixed">
+    <parent link="body"/>
+    <child link="AI_camera_link"/>
+    <origin rpy="0 0 0" xyz="290.228e-3 0 147.420e-3"/>
+  </joint>
+
+  <link name="AI_camera_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>
+    </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">
+    <origin rpy="0 0 0" xyz="${abad_offset_x} ${-abad_offset_y} 0"/>
+  </xacro:leg>
+
+  <xacro:leg name="FL" mirror="1" mirror_dae="True" front_hind="1" front_hind_dae="True">
+    <origin rpy="0 0 0" xyz="${abad_offset_x} ${abad_offset_y} 0"/>
+  </xacro:leg>
+
+  <xacro:leg name="RR" mirror="-1" mirror_dae="False" front_hind="-1" front_hind_dae="False">
+    <origin rpy="0 0 0" xyz="${-abad_offset_x} ${-abad_offset_y} 0"/>
+  </xacro:leg>
+
+  <xacro:leg name="RL" mirror="1" mirror_dae="True" front_hind="-1" front_hind_dae="False">
+    <origin rpy="0 0 0" xyz="${-abad_offset_x} ${abad_offset_y} 0"/>
+  </xacro:leg>
+
+  <!-- This link is only for head collision -->
+  <joint name="head_joint" type="fixed">
+    <parent link="body"/>
+    <child link="head"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+  </joint>
+  <link name="head">
+    <collision>
+      <origin rpy="0 0 0" xyz="0.256 0 0.120"/>
+      <geometry>
+        <box size="0.076 0.060 0.040"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.225 0 0.150"/>
+      <geometry>
+        <box size="0.020 0.080 0.100"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <gazebo reference="body">
+    <visual>
+      <material>
+        <ambient>.1 .1 .1 1.0</ambient>
+        <diffuse>.1 .1 .1 1.0</diffuse>
+        <specular>.1 .1 .1 1.0</specular>
+      </material>
+    </visual>
+  </gazebo>
+
+</robot>
\ No newline at end of file
diff --git a/descriptions/xiaomi/cyberdog_description/xacro/ros2_control.xacro b/descriptions/xiaomi/cyberdog_description/xacro/ros2_control.xacro
new file mode 100644
index 0000000..0f47197
--- /dev/null
+++ b/descriptions/xiaomi/cyberdog_description/xacro/ros2_control.xacro
@@ -0,0 +1,176 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <ros2_control name="UnitreeSystem" type="system">
+
+    <hardware>
+      <plugin>hardware_unitree_mujoco/HardwareUnitree</plugin>
+    </hardware>
+
+    <joint name="FR_hip_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="FR_thigh_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="FR_calf_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="FL_hip_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="FL_thigh_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="FL_calf_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="RR_hip_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="RR_thigh_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="RR_calf_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="RL_hip_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="RL_thigh_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="RL_calf_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <sensor name="imu_sensor">
+      <state_interface name="orientation.w"/>
+      <state_interface name="orientation.x"/>
+      <state_interface name="orientation.y"/>
+      <state_interface name="orientation.z"/>
+      <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>
+
+    <sensor name="foot_force">
+      <state_interface name="FR"/>
+      <state_interface name="FL"/>
+      <state_interface name="RR"/>
+      <state_interface name="RL"/>
+    </sensor>
+
+  </ros2_control>
+
+</robot>
diff --git a/descriptions/xiaomi/cyberdog_description/xacro/transmission.xacro b/descriptions/xiaomi/cyberdog_description/xacro/transmission.xacro
new file mode 100644
index 0000000..f016209
--- /dev/null
+++ b/descriptions/xiaomi/cyberdog_description/xacro/transmission.xacro
@@ -0,0 +1,41 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:macro name="leg_transmission" params="name">
+
+    <transmission name="${name}_abad_tran">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${name}_abad_joint">
+        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      </joint>
+      <actuator name="${name}_abad_motor">
+        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        <mechanicalReduction>"${abadGearRatio}"</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+    <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>"${abadGearRatio}"</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+    <transmission name="${name}_knee_tran">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${name}_knee_joint">
+        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      </joint>
+      <actuator name="${name}_knee_motor">
+        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        <mechanicalReduction>"${kneeGearRatio}"</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+  </xacro:macro>
+
+</robot>