feat: add more robots (b2 b2w go2w)

2025-02-15 18:47:11 +08:00 · 2025-02-15 18:47:11 +08:00 · 3e7306dfa1
parent 69d7f91bb7
commit 3e7306dfa1
90 changed files with 14768 additions and 0 deletions
--- a/src/robots/b2_description/CMakeLists.txt
+++ b/src/robots/b2_description/CMakeLists.txt
@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 2.8.3)
+
+project(b2_description)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+find_package(roslaunch)
+
+foreach(dir config launch meshes urdf)
+	install(DIRECTORY ${dir}/
+		DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
+endforeach(dir)
--- a/src/robots/b2_description/README.md
+++ b/src/robots/b2_description/README.md
@ -0,0 +1,43 @@
+## b2_urdf
+This repository contains the urdf model of b2.
+
+
+## Build the library
+Create a new catkin workspace:
+```
+# Create the directories
+# Do not forget to change <...> parts
+mkdir -p <directory_to_ws>/<catkin_ws_name>/src
+cd <directory_to_ws>/<catkin_ws_name>/
+
+# Initialize the catkin workspace
+catkin init
+```
+
+Clone library:
+```
+# Navigate to the directory of src
+# Do not forget to change <...> parts
+cd <directory_to_ws>/<catkin_ws_name>/src
+git clone git@github.com:unitreerobotics
+```
+
+Build:
+```
+# Build it
+catkin build
+
+# Source it
+source <directory_to_ws>/<catkin_ws_name>/devel/setup.bash
+```
+
+
+
+## Run the library
+```
+# Show urdf model of b2 in Rviz
+roslaunch b2_description display.launch
+
+```
+
+## support issac
--- a/src/robots/b2_description/config/config.rviz
+++ b/src/robots/b2_description/config/config.rviz
@ -0,0 +1,361 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 138
+    Name: Displays
+    Property Tree Widget:
+      Expanded: ~
+      Splitter Ratio: 0.8341270089149475
+    Tree Height: 1576
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 0.5
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        f_dc_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        f_oc_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        fl_c_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        fl_f_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        fl_h_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        fl_t_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        fr_c_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        fr_f_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        fr_h_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        fr_t_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        head_Link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        lidar_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        r_dc_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        r_oc_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rl_c_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rl_f_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rl_h_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rl_t_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rr_c_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rr_f_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rr_h_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rr_t_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        tail_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/TF
+      Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        base_link:
+          Value: true
+        f_dc_link:
+          Value: true
+        f_oc_link:
+          Value: true
+        fl_c_link:
+          Value: true
+        fl_f_link:
+          Value: true
+        fl_h_link:
+          Value: true
+        fl_t_link:
+          Value: true
+        fr_c_link:
+          Value: true
+        fr_f_link:
+          Value: true
+        fr_h_link:
+          Value: true
+        fr_t_link:
+          Value: true
+        head_Link:
+          Value: true
+        imu_link:
+          Value: true
+        lidar_link:
+          Value: true
+        r_dc_link:
+          Value: true
+        r_oc_link:
+          Value: true
+        rl_c_link:
+          Value: true
+        rl_f_link:
+          Value: true
+        rl_h_link:
+          Value: true
+        rl_t_link:
+          Value: true
+        rr_c_link:
+          Value: true
+        rr_f_link:
+          Value: true
+        rr_h_link:
+          Value: true
+        rr_t_link:
+          Value: true
+        tail_link:
+          Value: true
+      Marker Alpha: 1
+      Marker Scale: 0.4000000059604645
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        base_link:
+          f_dc_link:
+            {}
+          f_oc_link:
+            {}
+          fl_h_link:
+            fl_t_link:
+              fl_c_link:
+                fl_f_link:
+                  {}
+          fr_h_link:
+            fr_t_link:
+              fr_c_link:
+                fr_f_link:
+                  {}
+          head_Link:
+            {}
+          imu_link:
+            {}
+          lidar_link:
+            {}
+          r_dc_link:
+            {}
+          r_oc_link:
+            {}
+          rl_h_link:
+            rl_t_link:
+              rl_c_link:
+                rl_f_link:
+                  {}
+          rr_h_link:
+            rr_t_link:
+              rr_c_link:
+                rr_f_link:
+                  {}
+          tail_link:
+            {}
+      Update Interval: 0
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: base_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 1.8284358978271484
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: 0.01927083730697632
+        Y: -0.037570029497146606
+        Z: -0.34493619203567505
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.555398166179657
+      Target Frame: <Fixed Frame>
+      Yaw: 0.7953987121582031
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 2086
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000024000000720fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000007200000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f00000720fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000006e000007200000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000f000000005efc0100000002fb0000000800540069006d0065010000000000000f00000006dc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000cb40000072000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 3840
+  X: 3840
+  Y: 0
--- a/src/robots/b2_description/config/robot_control.yaml
+++ b/src/robots/b2_description/config/robot_control.yaml
@ -0,0 +1,61 @@
+# Copyright (c) 2024-2025 Ziqi Fan
+# SPDX-License-Identifier: Apache-2.0
+
+b2_gazebo:
+    # Publish all joint states -----------------------------------
+    joint_state_controller:
+        type: joint_state_controller/JointStateController
+        publish_rate: 1000
+
+    # FL Controllers ---------------------------------------
+    FL_hip_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: FL_hip_joint
+
+    FL_thigh_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: FL_thigh_joint
+
+    FL_calf_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: FL_calf_joint
+
+    # FR Controllers ---------------------------------------
+    FR_hip_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: FR_hip_joint
+
+    FR_thigh_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: FR_thigh_joint
+
+    FR_calf_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: FR_calf_joint
+
+    # RL Controllers ---------------------------------------
+    RL_hip_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: RL_hip_joint
+
+    RL_thigh_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: RL_thigh_joint
+
+    RL_calf_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: RL_calf_joint
+
+    # RR Controllers ---------------------------------------
+    RR_hip_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: RR_hip_joint
+
+    RR_thigh_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: RR_thigh_joint
+
+    RR_calf_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: RR_calf_joint
+
--- a/src/robots/b2_description/launch/display.launch
+++ b/src/robots/b2_description/launch/display.launch
@ -0,0 +1,20 @@
+<launch>
+  <arg
+    name="model" />
+  <param
+    name="robot_description"
+    textfile="$(find b2_description)/urdf/b2_description.urdf" />
+  <node
+    name="joint_state_publisher_gui"
+    pkg="joint_state_publisher_gui"
+    type="joint_state_publisher_gui" />
+  <node
+    name="robot_state_publisher"
+    pkg="robot_state_publisher"
+    type="robot_state_publisher" />
+  <node
+    name="rviz"
+    pkg="rviz"
+    type="rviz"
+    args="-d $(find b2_description)/config/config.rviz" />
+</launch>
--- a/src/robots/b2_description/launch/gazebo.launch
+++ b/src/robots/b2_description/launch/gazebo.launch
@ -0,0 +1,20 @@
+<launch>
+  <include
+    file="$(find gazebo_ros)/launch/empty_world.launch" />
+  <node
+    name="tf_footprint_base"
+    pkg="tf"
+    type="static_transform_publisher"
+    args="0 0 0 0 0 0 base_link base_footprint 40" />
+  <node
+    name="spawn_model"
+    pkg="gazebo_ros"
+    type="spawn_model"
+    args="-file $(find b2_description)/urdf/b2_description.urdf -urdf -model b2_description"
+    output="screen" />
+  <node
+    name="fake_joint_calibration"
+    pkg="rostopic"
+    type="rostopic"
+    args="pub /calibrated std_msgs/Bool true" />
+</launch>
--- a/src/robots/b2_description/meshes/FL_calf.dae
+++ b/src/robots/b2_description/meshes/FL_calf.dae
--- a/src/robots/b2_description/meshes/FL_foot.dae
+++ b/src/robots/b2_description/meshes/FL_foot.dae
--- a/src/robots/b2_description/meshes/FL_hip.dae
+++ b/src/robots/b2_description/meshes/FL_hip.dae
--- a/src/robots/b2_description/meshes/FL_thigh.dae
+++ b/src/robots/b2_description/meshes/FL_thigh.dae
--- a/src/robots/b2_description/meshes/FR_calf.dae
+++ b/src/robots/b2_description/meshes/FR_calf.dae
--- a/src/robots/b2_description/meshes/FR_hip.dae
+++ b/src/robots/b2_description/meshes/FR_hip.dae
--- a/src/robots/b2_description/meshes/FR_thigh.dae
+++ b/src/robots/b2_description/meshes/FR_thigh.dae
--- a/src/robots/b2_description/meshes/RL_calf.dae
+++ b/src/robots/b2_description/meshes/RL_calf.dae
--- a/src/robots/b2_description/meshes/RL_hip.dae
+++ b/src/robots/b2_description/meshes/RL_hip.dae
--- a/src/robots/b2_description/meshes/RL_thigh.dae
+++ b/src/robots/b2_description/meshes/RL_thigh.dae
--- a/src/robots/b2_description/meshes/RR_calf.dae
+++ b/src/robots/b2_description/meshes/RR_calf.dae
--- a/src/robots/b2_description/meshes/RR_hip.dae
+++ b/src/robots/b2_description/meshes/RR_hip.dae
--- a/src/robots/b2_description/meshes/RR_thigh.dae
+++ b/src/robots/b2_description/meshes/RR_thigh.dae
--- a/src/robots/b2_description/meshes/base_link.dae
+++ b/src/robots/b2_description/meshes/base_link.dae
--- a/src/robots/b2_description/meshes/calf.dae
+++ b/src/robots/b2_description/meshes/calf.dae
--- a/src/robots/b2_description/meshes/hip.dae
+++ b/src/robots/b2_description/meshes/hip.dae
--- a/src/robots/b2_description/meshes/thigh.dae
+++ b/src/robots/b2_description/meshes/thigh.dae
--- a/src/robots/b2_description/meshes/thigh_mirror.dae
+++ b/src/robots/b2_description/meshes/thigh_mirror.dae
--- a/src/robots/b2_description/meshes/trunk.dae
+++ b/src/robots/b2_description/meshes/trunk.dae
--- a/src/robots/b2_description/meshes/trunk1.dae
+++ b/src/robots/b2_description/meshes/trunk1.dae
--- a/src/robots/b2_description/meshes/trunk2.dae
+++ b/src/robots/b2_description/meshes/trunk2.dae
--- a/src/robots/b2_description/package.xml
+++ b/src/robots/b2_description/package.xml
@ -0,0 +1,21 @@
+<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>
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>roslaunch</depend>
+  <depend>robot_state_publisher</depend>
+  <depend>rviz</depend>
+  <depend>joint_state_publisher_gui</depend>
+  <depend>gazebo</depend>
+  <export>
+    <architecture_independent />
+  </export>
+</package>
--- a/src/robots/b2_description/urdf/b2_description.urdf
+++ b/src/robots/b2_description/urdf/b2_description.urdf
--- a/src/robots/b2_description/xacro/const.xacro
+++ b/src/robots/b2_description/xacro/const.xacro
@ -0,0 +1,160 @@
+<?xml version="1.0"?>
+
+<robot name="b2_description" xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <!-- Constants for robot dimensions -->
+    <xacro:property name="PI" value="3.1415926535897931"/>
+    <!-- <xacro:property name="stick_mass" value="0.00001"/> -->
+
+    <!-- simplified collision value -->
+    <xacro:property name="trunk_width" value="0.28"/>
+    <xacro:property name="trunk_length" value="0.5"/>
+    <xacro:property name="trunk_height" value="0.15"/>
+    <xacro:property name="hip_radius" value="0.07"/>
+    <xacro:property name="hip_length" value="0.05"/>
+    <!-- <xacro:property name="thigh_shoulder_radius" value="0.044"/>
+    <xacro:property name="thigh_shoulder_length" value="0.08"/> -->
+    <xacro:property name="thigh_width" value="0.0455"/>
+    <xacro:property name="thigh_height" value="0.054"/>
+    <!-- <xacro:property name="calf_width" value="0.04"/>
+    <xacro:property name="calf_height" value="0.05"/> -->
+    <xacro:property name="foot_radius" value="0.04"/>
+    <!-- <xacro:property name="stick_radius" value="0.01"/>
+    <xacro:property name="stick_length" value="0.2"/> -->
+
+    <!-- kinematic value -->
+    <xacro:property name="thigh_offset" value="0.11973"/>
+    <xacro:property name="thigh_length" value="0.32"/>
+    <xacro:property name="calf_length" value="0.35"/>
+
+    <!-- leg offset from trunk center value -->
+    <xacro:property name="leg_offset_x" value="0.3285"/>
+    <xacro:property name="leg_offset_y" value="0.072"/>
+    <!-- <xacro:property name="trunk_offset_z" value="0.0"/> -->
+    <xacro:property name="hip_offset" value="0.11973"/>
+
+    <!-- offset of link and rotor locations (left front) -->
+    <!-- <xacro:property name="hip_offset_x" value="0.3285"/>
+    <xacro:property name="hip_offset_y" value="0.072"/>
+    <xacro:property name="hip_offset_z" value="0.0"/> -->
+    <xacro:property name="hip_rotor_offset_x" value="0.20205"/>
+    <xacro:property name="hip_rotor_offset_y" value="0.072"/>
+    <xacro:property name="hip_rotor_offset_z" value="0.0"/>
+
+    <!-- <xacro:property name="thigh_offset_x" value="0"/>
+    <xacro:property name="thigh_offset_y" value="0.11973"/>
+    <xacro:property name="thigh_offset_z" value="0.0"/> -->
+    <xacro:property name="thigh_rotor_offset_x" value="0.0"/>
+    <xacro:property name="thigh_rotor_offset_y" value="0.00798"/>
+    <xacro:property name="thigh_rotor_offset_z" value="0.0"/>
+
+    <!-- <xacro:property name="calf_offset_x" value="0.0"/>
+    <xacro:property name="calf_offset_y" value="0.0"/>
+    <xacro:property name="calf_offset_z" value="-0.35"/> -->
+    <xacro:property name="calf_rotor_offset_x" value="0.0"/>
+    <xacro:property name="calf_rotor_offset_y" value="-0.05788"/>
+    <xacro:property name="calf_rotor_offset_z" value="0.0"/>
+
+
+    <!-- joint limits -->
+    <xacro:property name="damping" value="0"/>
+    <xacro:property name="friction" value="0"/>
+    <xacro:property name="hip_position_max" value="0.87"/>
+    <xacro:property name="hip_position_min" value="-0.87"/>
+    <xacro:property name="hip_velocity_max" value="23"/>
+    <xacro:property name="hip_torque_max" value="200"/>
+    <xacro:property name="thigh_position_max" value="4.69"/>
+    <xacro:property name="thigh_position_min" value="-94"/>
+    <xacro:property name="thigh_velocity_max" value="23"/>
+    <xacro:property name="thigh_torque_max" value="200"/>
+    <xacro:property name="calf_position_max" value="-0.43"/>
+    <xacro:property name="calf_position_min" value="-2.82"/>
+    <xacro:property name="calf_velocity_max" value="14"/>
+    <xacro:property name="calf_torque_max" value="320"/>
+
+    <!-- dynamics inertial value -->
+    <!-- trunk -->
+    <xacro:property name="trunk_mass" value="35.606"/>
+    <xacro:property name="trunk_com_x" value="0.000458"/>
+    <xacro:property name="trunk_com_y" value="0.005261"/>
+    <xacro:property name="trunk_com_z" value="0.000665"/>
+    <xacro:property name="trunk_ixx" value="0.27466"/>
+    <xacro:property name="trunk_ixy" value="-0.000622"/>
+    <xacro:property name="trunk_ixz" value="-0.00315"/>
+    <xacro:property name="trunk_iyy" value="1.0618"/>
+    <xacro:property name="trunk_iyz" value="-0.00139"/>
+    <xacro:property name="trunk_izz" value="1.1825"/>
+
+    <!-- hip (left front) -->
+    <xacro:property name="hip_mass" value="2.256"/>
+    <xacro:property name="hip_com_x" value="-0.009305"/>
+    <xacro:property name="hip_com_y" value="-0.010228"/>
+    <xacro:property name="hip_com_z" value="0.000264"/>
+    <xacro:property name="hip_ixx" value="0.0026431"/>
+    <xacro:property name="hip_ixy" value="0.00019234"/>
+    <xacro:property name="hip_ixz" value="-6.76E-06"/>
+    <xacro:property name="hip_iyy" value="0.0046728"/>
+    <xacro:property name="hip_iyz" value="-7.16E-06"/>
+    <xacro:property name="hip_izz" value="0.0034208"/>
+
+    <xacro:property name="hip_rotor_mass" value="0.2734"/>
+    <xacro:property name="hip_rotor_com_x" value="0.0"/>
+    <xacro:property name="hip_rotor_com_y" value="0.0"/>
+    <xacro:property name="hip_rotor_com_z" value="0.0"/>
+    <xacro:property name="hip_rotor_ixx" value="0.000144463"/>
+    <xacro:property name="hip_rotor_ixy" value="0.0"/>
+    <xacro:property name="hip_rotor_ixz" value="0.0"/>
+    <xacro:property name="hip_rotor_iyy" value="0.000144463"/>
+    <xacro:property name="hip_rotor_iyz" value="0.0"/>
+    <xacro:property name="hip_rotor_izz" value="0.000263053"/>
+
+    <!-- thigh -->
+    <xacro:property name="thigh_mass" value="3.591"/>
+    <xacro:property name="thigh_com_x" value="-0.004346"/>
+    <xacro:property name="thigh_com_y" value="-0.035797"/>
+    <xacro:property name="thigh_com_z" value="-0.044921"/>
+    <xacro:property name="thigh_ixx" value="0.041718"/>
+    <xacro:property name="thigh_ixy" value="0.00055623"/>
+    <xacro:property name="thigh_ixz" value="-0.0022768"/>
+    <xacro:property name="thigh_iyy" value="0.041071"/>
+    <xacro:property name="thigh_iyz" value="0.005796"/>
+    <xacro:property name="thigh_izz" value="0.0065677"/>
+
+    <xacro:property name="thigh_rotor_mass" value="0.2734"/>
+    <xacro:property name="thigh_rotor_com_x" value="0.0"/>
+    <xacro:property name="thigh_rotor_com_y" value="0.0"/>
+    <xacro:property name="thigh_rotor_com_z" value="0.0"/>
+    <xacro:property name="thigh_rotor_ixx" value="0.000144463"/>
+    <xacro:property name="thigh_rotor_ixy" value="0.0"/>
+    <xacro:property name="thigh_rotor_ixz" value="0.0"/>
+    <xacro:property name="thigh_rotor_iyy" value="0.000144463"/>
+    <xacro:property name="thigh_rotor_iyz" value="0.0"/>
+    <xacro:property name="thigh_rotor_izz" value="0.000263053"/>
+
+    <!-- calf -->
+    <xacro:property name="calf_mass" value="0.58094"/>
+    <xacro:property name="calf_com_x" value="0.012422"/>
+    <xacro:property name="calf_com_y" value="0.0"/>
+    <xacro:property name="calf_com_z" value="-0.12499"/>
+    <xacro:property name="calf_ixx" value="0.01143"/>
+    <xacro:property name="calf_ixy" value="0"/>
+    <xacro:property name="calf_ixz" value="0.000643"/>
+    <xacro:property name="calf_iyy" value="0.011534"/>
+    <xacro:property name="calf_iyz" value="0"/>
+    <xacro:property name="calf_izz" value="0.000331"/>
+
+    <xacro:property name="calf_rotor_mass" value="0.2734"/>
+    <xacro:property name="calf_rotor_com_x" value="0.0"/>
+    <xacro:property name="calf_rotor_com_y" value="0.0"/>
+    <xacro:property name="calf_rotor_com_z" value="0.0"/>
+    <xacro:property name="calf_rotor_ixx" value="0.000144463"/>
+    <xacro:property name="calf_rotor_ixy" value="0.0"/>
+    <xacro:property name="calf_rotor_ixz" value="0.0"/>
+    <xacro:property name="calf_rotor_iyy" value="0.000144463"/>
+    <xacro:property name="calf_rotor_iyz" value="0.0"/>
+    <xacro:property name="calf_rotor_izz" value="0.000263053"/>
+
+    <!-- foot -->
+    <xacro:property name="foot_mass" value="0.098183"/>
+
+</robot>
--- a/src/robots/b2_description/xacro/gazebo.xacro
+++ b/src/robots/b2_description/xacro/gazebo.xacro
@ -0,0 +1,266 @@
+<?xml version="1.0"?>
+<robot>
+    <!-- ros_control plugin -->
+    <gazebo>
+        <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
+            <robotNamespace>/b2_gazebo</robotNamespace>
+            <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+        </plugin>
+    </gazebo>
+
+    <!-- Show the trajectory of trunk center. -->
+    <gazebo>
+        <plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
+            <frequency>10</frequency>
+            <plot>
+                <link>base</link>
+                <pose>0 0 0 0 0 0</pose>
+                <material>Gazebo/Yellow</material>
+            </plot>
+        </plugin>
+    </gazebo>
+
+    <!-- Show the trajectory of foot. You can add another trajectory about another foot. -->
+    <!-- <gazebo>
+        <plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
+            <frequency>100</frequency>
+            <plot>
+                <link>FL_foot</link>
+                <pose>0 0 0 0 0 0</pose>
+                <material>Gazebo/Green</material>
+            </plot>
+        </plugin>
+    </gazebo> -->
+
+    <gazebo>
+        <plugin filename="libgazebo_ros_force.so" name="gazebo_ros_force">
+            <bodyName>trunk</bodyName>
+            <topicName>/apply_force/trunk</topicName>
+        </plugin>
+    </gazebo>
+
+    <gazebo reference="imu_link">
+        <gravity>true</gravity>
+        <sensor name="imu_sensor" type="imu">
+            <always_on>true</always_on>
+            <update_rate>1000</update_rate>
+            <visualize>true</visualize>
+            <topic>__default_topic__</topic>
+            <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
+                <topicName>trunk_imu</topicName>
+                <bodyName>imu_link</bodyName>
+                <updateRateHZ>1000.0</updateRateHZ>
+                <gaussianNoise>0.0</gaussianNoise>
+                <xyzOffset>0 0 0</xyzOffset>
+                <rpyOffset>0 0 0</rpyOffset>
+                <frameName>imu_link</frameName>
+            </plugin>
+            <pose>0 0 0 0 0 0</pose>
+        </sensor>
+    </gazebo>
+
+    <!-- Foot contacts. -->
+    <gazebo reference="FR_calf">
+        <sensor name="FR_foot_contact" type="contact">
+        <update_rate>100</update_rate>
+        <plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
+        <contact>
+            <collision>FR_calf_fixed_joint_lump__FR_foot_collision_1</collision>
+        </contact>
+        </sensor>
+    </gazebo>
+    <gazebo reference="FL_calf">
+        <sensor name="FL_foot_contact" type="contact">
+        <update_rate>100</update_rate>
+        <plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
+        <contact>
+            <collision>FL_calf_fixed_joint_lump__FL_foot_collision_1</collision>
+        </contact>
+        </sensor>
+    </gazebo>
+    <gazebo reference="RR_calf">
+        <sensor name="RR_foot_contact" type="contact">
+        <update_rate>100</update_rate>
+        <plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
+        <contact>
+            <collision>RR_calf_fixed_joint_lump__RR_foot_collision_1</collision>
+        </contact>
+        </sensor>
+    </gazebo>
+    <gazebo reference="RL_calf">
+        <sensor name="RL_foot_contact" type="contact">
+        <update_rate>100</update_rate>
+        <plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
+        <contact>
+            <collision>RL_calf_fixed_joint_lump__RL_foot_collision_1</collision>
+        </contact>
+        </sensor>
+    </gazebo>
+
+    <!-- Visualization of Foot contacts. -->
+    <!-- <gazebo reference="FR_foot">
+        <visual>
+            <plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
+                <topicName>FR_foot_contact</topicName>
+            </plugin>
+        </visual>
+    </gazebo>
+    <gazebo reference="FL_foot">
+        <visual>
+            <plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
+                <topicName>FL_foot_contact</topicName>
+            </plugin>
+        </visual>
+    </gazebo>
+    <gazebo reference="RR_foot">
+        <visual>
+            <plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
+                <topicName>RR_foot_contact</topicName>
+            </plugin>
+        </visual>
+    </gazebo>
+    <gazebo reference="RL_foot">
+        <visual>
+            <plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
+                <topicName>RL_foot_contact</topicName>
+            </plugin>
+        </visual>
+    </gazebo> -->
+
+    <gazebo reference="base">
+        <material>Gazebo/Green</material>
+        <turnGravityOff>false</turnGravityOff>
+    </gazebo>
+
+    <gazebo reference="trunk">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+
+    <gazebo reference="stick_link">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <material>Gazebo/White</material>
+    </gazebo>
+
+    <gazebo reference="imu_link">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <material>Gazebo/Red</material>
+    </gazebo>
+
+    <!-- FL leg -->
+    <gazebo reference="FL_hip">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <!-- <material>Gazebo/DarkGrey</material> -->
+    </gazebo>
+    <gazebo reference="FL_thigh">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <self_collide>1</self_collide>
+        <!-- <material>Gazebo/DarkGrey</material> -->
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+    <gazebo reference="FL_calf">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <self_collide>1</self_collide>
+    </gazebo>
+    <gazebo reference="FL_foot">
+        <mu1>0.6</mu1>
+        <mu2>0.6</mu2>
+        <self_collide>1</self_collide>
+        <!-- <material>Gazebo/DarkGrey</material> -->
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+
+    <!-- FR leg -->
+    <gazebo reference="FR_hip">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <!-- <material>Gazebo/DarkGrey</material> -->
+    </gazebo>
+    <gazebo reference="FR_thigh">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <self_collide>1</self_collide>
+        <!-- <material>Gazebo/DarkGrey</material> -->
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+    <gazebo reference="FR_calf">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <self_collide>1</self_collide>
+    </gazebo>
+    <gazebo reference="FR_foot">
+        <mu1>0.6</mu1>
+        <mu2>0.6</mu2>
+        <self_collide>1</self_collide>
+        <!-- <material>Gazebo/DarkGrey</material> -->
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+
+    <!-- RL leg -->
+    <gazebo reference="RL_hip">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <!-- <material>Gazebo/DarkGrey</material> -->
+    </gazebo>
+    <gazebo reference="RL_thigh">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <self_collide>1</self_collide>
+        <!-- <material>Gazebo/DarkGrey</material> -->
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+    <gazebo reference="RL_calf">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <self_collide>1</self_collide>
+    </gazebo>
+    <gazebo reference="RL_foot">
+        <mu1>0.6</mu1>
+        <mu2>0.6</mu2>
+        <self_collide>1</self_collide>
+        <!-- <material>Gazebo/DarkGrey</material> -->
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+
+    <!-- RR leg -->
+    <gazebo reference="RR_hip">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <!-- <material>Gazebo/DarkGrey</material> -->
+    </gazebo>
+    <gazebo reference="RR_thigh">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <self_collide>1</self_collide>
+        <!-- <material>Gazebo/DarkGrey</material> -->
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+    <gazebo reference="RR_calf">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <self_collide>1</self_collide>
+    </gazebo>
+    <gazebo reference="RR_foot">
+        <mu1>0.6</mu1>
+        <mu2>0.6</mu2>
+        <self_collide>1</self_collide>
+        <!-- <material>Gazebo/DarkGrey</material> -->
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+
+</robot>
--- a/src/robots/b2_description/xacro/leg.xacro
+++ b/src/robots/b2_description/xacro/leg.xacro
@ -0,0 +1,267 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <xacro:include filename="$(find b2_description)/xacro/transmission.xacro"/>
+
+    <xacro:macro name="leg" params="name mirror mirror_dae front_hind front_hind_dae">
+
+        <joint name="${name}_hip_joint" type="revolute">
+            <xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
+                <origin rpy="0 0 0" xyz="${leg_offset_x} ${-leg_offset_y} 0"/>
+            </xacro:if>
+            <xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
+                <origin rpy="0 0 0" xyz="${leg_offset_x} ${leg_offset_y} 0"/>
+            </xacro:if>
+            <xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
+                <origin rpy="0 0 0" xyz="${-leg_offset_x} ${-leg_offset_y} 0"/>
+            </xacro:if>
+            <xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
+                <origin rpy="0 0 0" xyz="${-leg_offset_x} ${leg_offset_y} 0"/>
+            </xacro:if>
+            <parent link="trunk"/>
+            <child link="${name}_hip"/>
+            <axis xyz="1 0 0"/>
+            <dynamics damping="${damping}" friction="${friction}"/>
+            <xacro:if value="${(mirror_dae == True)}">
+                <limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${hip_position_min}" upper="${hip_position_max}"/>
+            </xacro:if>
+            <xacro:if value="${(mirror_dae == False)}">
+                <limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${-hip_position_max}" upper="${-hip_position_min}"/>
+            </xacro:if>
+        </joint>
+
+        <joint name="${name}_hip_rotor_joint" type="fixed">
+            <xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
+                <origin rpy="0 0 0" xyz="${hip_rotor_offset_x} ${-hip_rotor_offset_y} 0"/>
+            </xacro:if>
+            <xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
+                <origin rpy="0 0 0" xyz="${hip_rotor_offset_x} ${hip_rotor_offset_y} 0"/>
+            </xacro:if>
+            <xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
+                <origin rpy="0 0 0" xyz="${-hip_rotor_offset_x} ${-hip_rotor_offset_y} 0"/>
+            </xacro:if>
+            <xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
+                <origin rpy="0 0 0" xyz="${-hip_rotor_offset_x} ${hip_rotor_offset_y} 0"/>
+            </xacro:if>
+            <parent link="trunk"/>
+            <child link="${name}_hip_rotor"/>
+        </joint>
+
+        <link name="${name}_hip">
+            <visual>
+                <xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
+                    <origin rpy="0 0 0" xyz="0 0 0"/>
+                </xacro:if>
+                <xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
+                    <origin rpy="${PI} 0 0" xyz="0 0 0"/>
+                </xacro:if>
+                <xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
+                    <origin rpy="0 ${PI} 0" xyz="0 0 0"/>
+                </xacro:if>
+                <xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
+                    <origin rpy="${PI} ${PI} 0" xyz="0 0 0"/>
+                </xacro:if>
+                <geometry>
+                    <mesh filename="package://b2_description/meshes/hip.dae" scale="1 1 1"/>
+                </geometry>
+                <material name="orange"/>
+            </visual>
+            <collision>
+                <origin rpy="${PI/2.0} 0 0" xyz="0 ${hip_offset*mirror} 0"/>
+                <geometry>
+                    <cylinder length="${hip_length}" radius="${hip_radius}"/>
+                </geometry>
+            </collision>
+            <inertial>
+                <origin rpy="0 0 0" xyz="${hip_com_x*front_hind} ${hip_com_y*mirror} ${hip_com_z}"/>
+                <mass value="${hip_mass}"/>
+                <inertia
+                    ixx="${hip_ixx}" ixy="${hip_ixy*mirror*front_hind}" ixz="${hip_ixz*front_hind}"
+                    iyy="${hip_iyy}" iyz="${hip_iyz*mirror}"
+                    izz="${hip_izz}"/>
+            </inertial>
+        </link>
+
+        <link name="${name}_hip_rotor">
+            <visual>
+                <origin rpy="0 ${PI/2.0} 0" xyz="0 0 0"/>
+                <geometry>
+                    <cylinder length="0.02" radius="0.05"/>
+                </geometry>
+                <material name="green"/>
+            </visual>
+            <inertial>
+                <origin rpy="0 0 0" xyz="${hip_rotor_com_x} ${hip_rotor_com_y} ${hip_rotor_com_z}"/>
+                <mass value="${hip_rotor_mass}"/>
+                <inertia
+                    ixx="${hip_rotor_ixx}" ixy="${hip_rotor_ixy}" ixz="${hip_rotor_ixz}"
+                    iyy="${hip_rotor_iyy}" iyz="${hip_rotor_iyz}"
+                    izz="${hip_rotor_izz}"/>
+            </inertial>
+        </link>
+
+        <joint name="${name}_thigh_joint" type="revolute">
+            <origin rpy="0 0 0" xyz="0 ${thigh_offset*mirror} 0"/>
+            <parent link="${name}_hip"/>
+            <child link="${name}_thigh"/>
+            <axis xyz="0 1 0"/>
+            <dynamics damping="${damping}" friction="${friction}"/>
+            <limit effort="${thigh_torque_max}" velocity="${thigh_velocity_max}" lower="${thigh_position_min}" upper="${thigh_position_max}"/>
+        </joint>
+
+        <joint name="${name}_thigh_rotor_joint" type="fixed">
+            <origin rpy="0 0 0" xyz="${thigh_rotor_offset_x} ${thigh_rotor_offset_y*mirror} 0"/>
+            <parent link="${name}_hip"/>
+            <child link="${name}_thigh_rotor"/>
+        </joint>
+
+        <link name="${name}_thigh">
+            <visual>
+                <origin rpy="0 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <xacro:if value="${mirror_dae == True}">
+                        <mesh filename="package://b2_description/meshes/thigh.dae" scale="1 1 1"/>
+                    </xacro:if>
+                    <xacro:if value="${mirror_dae == False}">
+                        <mesh filename="package://b2_description/meshes/thigh_mirror.dae" scale="1 1 1"/>
+                    </xacro:if>
+                </geometry>
+                <material name="orange"/>
+            </visual>
+            <collision>
+                <origin rpy="0 ${PI/2.0} 0" xyz="-0.025 0 ${-thigh_length/2.0}"/>
+                <geometry>
+                    <box size="${thigh_length} ${thigh_width} ${thigh_height}"/>
+                </geometry>
+            </collision>
+            <inertial>
+                <origin rpy="0 0 0" xyz="${thigh_com_x} ${thigh_com_y*mirror} ${thigh_com_z}"/>
+                <mass value="${thigh_mass}"/>
+                <inertia
+                    ixx="${thigh_ixx}" ixy="${thigh_ixy*mirror}" ixz="${thigh_ixz}"
+                    iyy="${thigh_iyy}" iyz="${thigh_iyz*mirror}"
+                    izz="${thigh_izz}"/>
+            </inertial>
+        </link>
+
+        <link name="${name}_thigh_rotor">
+            <visual>
+                <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <cylinder length="0.02" radius="0.05"/>
+                </geometry>
+                <material name="green"/>
+            </visual>
+            <inertial>
+                <origin rpy="0 0 0" xyz="${thigh_rotor_com_x} ${thigh_rotor_com_y} ${thigh_rotor_com_z}"/>
+                <mass value="${thigh_rotor_mass}"/>
+                <inertia
+                    ixx="${thigh_rotor_ixx}" ixy="${thigh_rotor_ixy}" ixz="${thigh_rotor_ixz}"
+                    iyy="${thigh_rotor_iyy}" iyz="${thigh_rotor_iyz}"
+                    izz="${thigh_rotor_izz}"/>
+            </inertial>
+        </link>
+
+        <joint name="${name}_calf_joint" type="revolute">
+            <origin rpy="0 0 0" xyz="0 0 ${-thigh_length}"/>
+            <parent link="${name}_thigh"/>
+            <child link="${name}_calf"/>
+            <axis xyz="0 1 0"/>
+            <dynamics damping="${damping}" friction="${friction}"/>
+            <limit effort="${calf_torque_max}" velocity="${calf_velocity_max}" lower="${calf_position_min}" upper="${calf_position_max}"/>
+        </joint>
+
+        <joint name="${name}_calf_rotor_joint" type="fixed">
+            <origin rpy="0 0 0" xyz="${calf_rotor_offset_x} ${calf_rotor_offset_y*mirror} 0"/>
+            <parent link="${name}_thigh"/>
+            <child link="${name}_calf_rotor"/>
+        </joint>
+
+        <link name="${name}_calf">
+            <visual>
+                <origin rpy="0 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <mesh filename="package://b2_description/meshes/calf.dae" scale="1 1 1"/>
+                </geometry>
+                <material name="orange"/>
+            </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>
+            <inertial>
+                <origin rpy="0 0 0" xyz="${calf_com_x} ${calf_com_y} ${calf_com_z}"/>
+                <mass value="${calf_mass}"/>
+                <inertia
+                    ixx="${calf_ixx}" ixy="${calf_ixy}" ixz="${calf_ixz}"
+                    iyy="${calf_iyy}" iyz="${calf_iyz}"
+                    izz="${calf_izz}"/>
+            </inertial>
+        </link>
+
+        <link name="${name}_calf_rotor">
+            <visual>
+                <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <cylinder length="0.02" radius="0.05"/>
+                </geometry>
+                <material name="green"/>
+            </visual>
+            <inertial>
+                <origin rpy="0 0 0" xyz="${calf_rotor_com_x} ${calf_rotor_com_y} ${calf_rotor_com_z}"/>
+                <mass value="${calf_rotor_mass}"/>
+                <inertia
+                    ixx="${calf_rotor_ixx}" ixy="${calf_rotor_ixy}" ixz="${calf_rotor_ixz}"
+                    iyy="${calf_rotor_iyy}" iyz="${calf_rotor_iyz}"
+                    izz="${calf_rotor_izz}"/>
+            </inertial>
+        </link>
+
+        <joint name="${name}_foot_fixed" type="fixed">
+            <origin rpy="1.57 0 0" xyz="0 0 ${-(calf_length)}"/>
+            <parent link="${name}_calf"/>
+            <child link="${name}_foot"/>
+        </joint>
+
+        <link name="${name}_foot">
+            <collision>
+            <origin
+                xyz="0 0 0"
+                rpy="0 0 0" />
+            <geometry>
+                <sphere radius="0.032" />
+            </geometry>
+            </collision>
+            <inertial>
+                <mass value="${foot_mass}"/>
+                <inertia
+                    ixx="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" ixy="0.0" ixz="0.0"
+                    iyy="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" iyz="0.0"
+                    izz="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}"/>
+            </inertial>
+        </link>
+
+        <xacro:leg_transmission name="${name}"/>
+    </xacro:macro>
+</robot>
--- a/src/robots/b2_description/xacro/materials.xacro
+++ b/src/robots/b2_description/xacro/materials.xacro
@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<robot>
+
+    <material name="black">
+        <color rgba="0.0 0.0 0.0 1.0"/>
+    </material>
+
+    <material name="blue">
+        <color rgba="0.0 0.0 0.8 1.0"/>
+    </material>
+
+    <material name="green">
+        <color rgba="0.0 0.8 0.0 1.0"/>
+    </material>
+
+    <material name="grey">
+        <color rgba="0.2 0.2 0.2 1.0"/>
+    </material>
+
+    <material name="silver">
+        <color rgba="${233/255} ${233/255} ${216/255} 1.0"/>
+    </material>
+
+    <material name="orange">
+        <color rgba="${255/255} ${108/255} ${10/255} 1.0"/>
+    </material>
+
+    <material name="brown">
+        <color rgba="${222/255} ${207/255} ${195/255} 1.0"/>
+    </material>
+
+    <material name="red">
+        <color rgba="0.8 0.0 0.0 1.0"/>
+    </material>
+
+    <material name="white">
+        <color rgba="1.0 1.0 1.0 1.0"/>
+    </material>
+
+</robot>
--- a/src/robots/b2_description/xacro/robot.xacro
+++ b/src/robots/b2_description/xacro/robot.xacro
@ -0,0 +1,182 @@
+<?xml version="1.0"?>
+
+<robot name="b2_description" xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <xacro:include filename="$(find b2_description)/xacro/const.xacro"/>
+    <xacro:include filename="$(find b2_description)/xacro/materials.xacro"/>
+    <xacro:include filename="$(find b2_description)/xacro/leg.xacro"/>
+    <xacro:include filename="$(find b2_description)/xacro/gazebo.xacro"/>
+
+    <!-- Rotor related joint and link is only for demonstrate location. -->
+    <!-- Actually, the rotor will rotate and the joint is not fixed. Reduction ratio should be considered. -->
+
+    <!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
+    <xacro:if value="$(arg DEBUG)">
+        <link name="world"/>
+        <joint name="base_static_joint" type="fixed">
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <parent link="world"/>
+            <child link="base"/>
+        </joint>
+    </xacro:if>
+
+    <link name="base">
+        <visual>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <geometry>
+                <box size="0.001 0.001 0.001"/>
+            </geometry>
+        </visual>
+    </link>
+
+    <joint name="floating_base" type="fixed">
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <parent link="base"/>
+        <child link="trunk"/>
+    </joint>
+
+    <link name="trunk">
+        <visual>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <geometry>
+                <mesh filename="package://b2_description/meshes/trunk.dae" scale="1 1 1"/>
+            </geometry>
+            <!-- <material name="orange"/> -->
+        </visual>
+        <collision>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <geometry>
+                <box size="${trunk_length} ${trunk_width} ${trunk_height}"/>
+            </geometry>
+        </collision>
+        <!-- <collision>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <geometry>
+                <mesh filename="package://b1_description/meshes/trunk.dae" scale="1 1 1"/>
+            </geometry>
+        </collision> -->
+        <inertial>
+            <origin rpy="0 0 0" xyz="${trunk_com_x} ${trunk_com_y} ${trunk_com_z}"/>
+            <mass value="${trunk_mass}"/>
+            <inertia
+                ixx="${trunk_ixx}" ixy="${trunk_ixy}" ixz="${trunk_ixz}"
+                iyy="${trunk_iyy}" iyz="${trunk_iyz}"
+                izz="${trunk_izz}"/>
+        </inertial>
+    </link>
+
+    <joint name="imu_joint" type="fixed">
+        <parent link="trunk"/>
+        <child link="imu_link"/>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+    </joint>
+
+    <link name="imu_link">
+        <inertial>
+            <mass value="0.001"/>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
+        </inertial>
+        <visual>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <geometry>
+                <box size="0.001 0.001 0.001"/>
+            </geometry>
+            <!-- <material name="red"/> -->
+        </visual>
+        <collision>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <geometry>
+                <box size=".001 .001 .001"/>
+            </geometry>
+        </collision>
+    </link>
+
+    <joint name="head_joint" type="fixed">
+        <parent link="trunk"/>
+        <child link="head_Link"/>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+    </joint>
+
+    <link name="head_Link">
+        <inertial>
+            <origin rpy="0 0 0" xyz="0.33989 -0.000168 0.12029"/>
+            <mass value="3.366" />
+            <inertia
+            ixx="0.026455"
+            ixy="3.15E-05"
+            ixz="0.00166"
+            iyy="0.029221"
+            iyz="-3.27E-05"
+            izz="0.010918" />
+        </inertial>
+        <collision>
+            <origin rpy="0 0 0" xyz="0.41 0 0.005"/>
+            <geometry>
+                <box size="0.04 0.12 0.14" />
+            </geometry>
+        </collision>
+    </link>
+
+    <joint name="lidar_joint" type="fixed">
+        <parent link="trunk"/>
+        <child link="lidar_link"/>
+        <origin rpy="0 0 0" xyz="0.34218 0 0.17851"/>
+    </joint>
+
+    <link name="lidar_link">
+        <inertial>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <mass value="6.55E-08" />
+            <inertia
+            ixx="1.64E-15"
+            ixy="0"
+            ixz="-1.09E-47"
+            iyy="1.64E-15"
+            iyz="-1.32E-47"
+            izz="1.64E-15" />
+        </inertial>
+        <collision>
+            <origin rpy="0 0 0" xyz="0 0 -0.02"/>
+            <geometry>
+                <cylinder length="0.16" radius="0.076" />
+            </geometry>
+        </collision>
+    </link>
+
+    <joint name="tail_joint" type="fixed">
+        <parent link="trunk"/>
+        <child link="tail_link"/>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+    </joint>
+
+    <link name="tail_link">
+        <inertial>
+            <origin rpy="0 0 0" xyz="-0.37448 0.000488 0.006495"/>
+            <mass value="0.777" />
+            <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 rpy="0 0 0" xyz="-0.405 0 0.005"/>
+            <geometry>
+                <box size="0.025 0.12 0.14" />
+            </geometry>
+        </collision>
+    </link>
+
+
+
+    <xacro:leg name="FR" mirror="-1" mirror_dae= "False" front_hind="1"  front_hind_dae="True"  />
+    <xacro:leg name="FL" mirror="1"  mirror_dae="True"   front_hind="1"  front_hind_dae="True"  />
+    <xacro:leg name="RR" mirror="-1" mirror_dae="False"  front_hind="-1" front_hind_dae="False" />
+    <xacro:leg name="RL" mirror="1"  mirror_dae="True"   front_hind="-1" front_hind_dae="False" />
+
+    <!-- <xacro:stairs stairs="15" xpos="0" ypos="2.0" zpos="0" /> -->
+
+</robot>
--- a/src/robots/b2_description/xacro/stairs.xacro
+++ b/src/robots/b2_description/xacro/stairs.xacro
@ -0,0 +1,46 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <xacro:property name="stair_length" value="0.640" />
+    <xacro:property name="stair_width" value="0.310" />
+    <xacro:property name="stair_height" value="0.170" />
+
+    <xacro:macro name="stairs" params="stairs xpos ypos zpos">
+
+        <joint name="stair_joint_origin" type="fixed">
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <parent link="world"/>
+            <child link="stair_link_${stairs}"/>
+        </joint>
+
+        <link name="stair_link_${stairs}">
+            <visual>
+                <geometry>
+                    <box size="${stair_length} ${stair_width} ${stair_height}"/>
+                </geometry>
+                <material name="grey"/>
+                <origin rpy="0 0 0" xyz="${xpos} ${ypos} ${zpos}"/>
+            </visual>
+            <collision>
+                <geometry>
+                    <box size="${stair_length} ${stair_width} ${stair_height}"/>
+                </geometry>
+            </collision>
+            <inertial>
+                <mass value="0.80"/>
+                <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+            </inertial>
+        </link>
+
+        <xacro:if value="${stairs}">
+            <xacro:stairs stairs="${stairs-1}" xpos="0" ypos="${ypos-stair_width/2}" zpos="${zpos+stair_height}"/>
+            <joint name="stair_joint_${stairs}" type="fixed">
+                <parent link="stair_link_${stairs}"/>
+                <child link="stair_link_${stairs-1}"/>
+            </joint>
+        </xacro:if>
+
+    </xacro:macro>
+
+</robot>
--- a/src/robots/b2_description/xacro/transmission.xacro
+++ b/src/robots/b2_description/xacro/transmission.xacro
@ -0,0 +1,42 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <xacro:macro name="leg_transmission" params="name">
+
+        <transmission name="${name}_hip_tran">
+            <type>transmission_interface/SimpleTransmission</type>
+            <joint name="${name}_hip_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            </joint>
+            <actuator name="${name}_hip_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+            </actuator>
+        </transmission>
+
+        <transmission name="${name}_thigh_tran">
+            <type>transmission_interface/SimpleTransmission</type>
+            <joint name="${name}_thigh_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            </joint>
+            <actuator name="${name}_thigh_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+            </actuator>
+        </transmission>
+
+        <transmission name="${name}_calf_tran">
+            <type>transmission_interface/SimpleTransmission</type>
+            <joint name="${name}_calf_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            </joint>
+            <actuator name="${name}_calf_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+            </actuator>
+        </transmission>
+
+    </xacro:macro>
+
+</robot>
--- a/src/robots/b2w_description/CMakeLists.txt
+++ b/src/robots/b2w_description/CMakeLists.txt
@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 2.8.3)
+
+project(b2w_description)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+find_package(roslaunch)
+
+foreach(dir config launch meshes urdf)
+	install(DIRECTORY ${dir}/
+		DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
+endforeach(dir)
--- a/src/robots/b2w_description/README.md
+++ b/src/robots/b2w_description/README.md
@ -0,0 +1,43 @@
+## b2w_urdf
+This repository contains the urdf model of b2w.
+
+
+## Build the library
+Create a new catkin workspace:
+```
+# Create the directories
+# Do not forget to change <...> parts
+mkdir -p <directory_to_ws>/<catkin_ws_name>/src
+cd <directory_to_ws>/<catkin_ws_name>/
+
+# Initialize the catkin workspace
+catkin init
+```
+
+Clone library:
+```
+# Navigate to the directory of src
+# Do not forget to change <...> parts
+cd <directory_to_ws>/<catkin_ws_name>/src
+git clone git@github.com:unitreerobotics
+```
+
+Build:
+```
+# Build it
+catkin build
+
+# Source it
+source <directory_to_ws>/<catkin_ws_name>/devel/setup.bash
+```
+
+
+
+## Run the library
+```
+# Show urdf model of b2w in Rviz
+roslaunch b2w_description display.launch
+
+```
+
+## support issac
--- a/src/robots/b2w_description/config/b2w.rviz
+++ b/src/robots/b2w_description/config/b2w.rviz
@ -0,0 +1,259 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+      Splitter Ratio: 0.5
+    Tree Height: 1079
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        FL_calf:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FL_foot:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FL_hip:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FL_thigh:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_calf:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_foot:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_hip:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_thigh:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        Link Tree Style: Links in Alphabetic Order
+        RL_calf:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RL_foot:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RL_hip:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RL_thigh:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RR_calf:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RR_foot:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RR_hip:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RR_thigh:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        f_dc_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        f_oc_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        head_Link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        lidar_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        logo_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        r_dc_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        r_oc_ink:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        tail_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: base_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 2.1567115783691406
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: 0.1668100506067276
+        Y: 0.024347694590687752
+        Z: -0.19685064256191254
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.38039839267730713
+      Target Frame: <Fixed Frame>
+      Yaw: 1.0103977918624878
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1376
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000004c2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004c2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004c2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000004c2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b80000003efc0100000002fb0000000800540069006d00650100000000000009b8000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000747000004c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 2488
+  X: 72
+  Y: 27
--- a/src/robots/b2w_description/config/robot_control.yaml
+++ b/src/robots/b2w_description/config/robot_control.yaml
@ -0,0 +1,76 @@
+# Copyright (c) 2024-2025 Ziqi Fan
+# SPDX-License-Identifier: Apache-2.0
+
+b2w_gazebo:
+    # Publish all joint states -----------------------------------
+    joint_state_controller:
+        type: joint_state_controller/JointStateController
+        publish_rate: 1000
+
+    # FL Controllers ---------------------------------------
+    FL_hip_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: FL_hip_joint
+
+    FL_thigh_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: FL_thigh_joint
+
+    FL_calf_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: FL_calf_joint
+
+    FL_foot_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: FL_foot_joint
+
+    # FR Controllers ---------------------------------------
+    FR_hip_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: FR_hip_joint
+
+    FR_thigh_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: FR_thigh_joint
+
+    FR_calf_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: FR_calf_joint
+
+    FR_foot_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: FR_foot_joint
+
+    # RL Controllers ---------------------------------------
+    RL_hip_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: RL_hip_joint
+
+    RL_thigh_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: RL_thigh_joint
+
+    RL_calf_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: RL_calf_joint
+
+    RL_foot_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: RL_foot_joint
+
+    # RR Controllers ---------------------------------------
+    RR_hip_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: RR_hip_joint
+
+    RR_thigh_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: RR_thigh_joint
+
+    RR_calf_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: RR_calf_joint
+
+    RR_foot_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: RR_foot_joint
--- a/src/robots/b2w_description/launch/display.launch
+++ b/src/robots/b2w_description/launch/display.launch
@ -0,0 +1,20 @@
+<launch>
+  <arg
+    name="model" />
+  <param
+    name="robot_description"
+    textfile="$(find b2w_description)/urdf/b2w_description.urdf" />
+  <node
+    name="joint_state_publisher_gui"
+    pkg="joint_state_publisher_gui"
+    type="joint_state_publisher_gui" />
+  <node
+    name="robot_state_publisher"
+    pkg="robot_state_publisher"
+    type="robot_state_publisher" />
+  <node
+    name="rviz"
+    pkg="rviz"
+    type="rviz"
+    args="-d $(find b2w_description)/config/b2w.rviz" />
+</launch>
--- a/src/robots/b2w_description/launch/gazebo.launch
+++ b/src/robots/b2w_description/launch/gazebo.launch
@ -0,0 +1,20 @@
+<launch>
+  <include
+    file="$(find gazebo_ros)/launch/empty_world.launch" />
+  <node
+    name="tf_footprint_base"
+    pkg="tf"
+    type="static_transform_publisher"
+    args="0 0 0 0 0 0 base_link base_footprint 40" />
+  <node
+    name="spawn_model"
+    pkg="gazebo_ros"
+    type="spawn_model"
+    args="-file $(find b2w_description)/urdf/b2w_description.urdf -urdf -model b2w_description"
+    output="screen" />
+  <node
+    name="fake_joint_calibration"
+    pkg="rostopic"
+    type="rostopic"
+    args="pub /calibrated std_msgs/Bool true" />
+</launch>
--- a/src/robots/b2w_description/meshes/FL_calf.dae
+++ b/src/robots/b2w_description/meshes/FL_calf.dae
--- a/src/robots/b2w_description/meshes/FL_foot.dae
+++ b/src/robots/b2w_description/meshes/FL_foot.dae
--- a/src/robots/b2w_description/meshes/FL_hip.dae
+++ b/src/robots/b2w_description/meshes/FL_hip.dae
--- a/src/robots/b2w_description/meshes/FL_thigh.dae
+++ b/src/robots/b2w_description/meshes/FL_thigh.dae
--- a/src/robots/b2w_description/meshes/FR_calf.dae
+++ b/src/robots/b2w_description/meshes/FR_calf.dae
--- a/src/robots/b2w_description/meshes/FR_foot.dae
+++ b/src/robots/b2w_description/meshes/FR_foot.dae
--- a/src/robots/b2w_description/meshes/FR_hip.dae
+++ b/src/robots/b2w_description/meshes/FR_hip.dae
--- a/src/robots/b2w_description/meshes/FR_thigh.dae
+++ b/src/robots/b2w_description/meshes/FR_thigh.dae
--- a/src/robots/b2w_description/meshes/RL_calf.dae
+++ b/src/robots/b2w_description/meshes/RL_calf.dae
--- a/src/robots/b2w_description/meshes/RL_foot.dae
+++ b/src/robots/b2w_description/meshes/RL_foot.dae
--- a/src/robots/b2w_description/meshes/RL_hip.dae
+++ b/src/robots/b2w_description/meshes/RL_hip.dae
--- a/src/robots/b2w_description/meshes/RL_thigh.dae
+++ b/src/robots/b2w_description/meshes/RL_thigh.dae
--- a/src/robots/b2w_description/meshes/RR_calf.dae
+++ b/src/robots/b2w_description/meshes/RR_calf.dae
--- a/src/robots/b2w_description/meshes/RR_foot.dae
+++ b/src/robots/b2w_description/meshes/RR_foot.dae
--- a/src/robots/b2w_description/meshes/RR_hip.dae
+++ b/src/robots/b2w_description/meshes/RR_hip.dae
--- a/src/robots/b2w_description/meshes/RR_thigh.dae
+++ b/src/robots/b2w_description/meshes/RR_thigh.dae
--- a/src/robots/b2w_description/meshes/base_link.dae
+++ b/src/robots/b2w_description/meshes/base_link.dae
--- a/src/robots/b2w_description/meshes/calf.dae
+++ b/src/robots/b2w_description/meshes/calf.dae
--- a/src/robots/b2w_description/meshes/calf_mirror.dae
+++ b/src/robots/b2w_description/meshes/calf_mirror.dae
--- a/src/robots/b2w_description/meshes/hip.dae
+++ b/src/robots/b2w_description/meshes/hip.dae
--- a/src/robots/b2w_description/meshes/thigh.dae
+++ b/src/robots/b2w_description/meshes/thigh.dae
--- a/src/robots/b2w_description/meshes/thigh_mirror.dae
+++ b/src/robots/b2w_description/meshes/thigh_mirror.dae
--- a/src/robots/b2w_description/meshes/trunk.dae
+++ b/src/robots/b2w_description/meshes/trunk.dae
--- a/src/robots/b2w_description/package.xml
+++ b/src/robots/b2w_description/package.xml
@ -0,0 +1,21 @@
+<package format="2">
+  <name>b2w_description</name>
+  <version>1.0.0</version>
+  <description>
+    <p>URDF Description package for b2w_description</p>
+    <p>This package contains configuration data, 3D models and launch files
+for b2w_description robot</p>
+  </description>
+  <author>TODO</author>
+  <maintainer email="TODO@email.com" />
+  <license>BSD</license>
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>roslaunch</depend>
+  <depend>robot_state_publisher</depend>
+  <depend>rviz</depend>
+  <depend>joint_state_publisher_gui</depend>
+  <depend>gazebo</depend>
+  <export>
+    <architecture_independent />
+  </export>
+</package>
--- a/src/robots/b2w_description/urdf/b2w_description.urdf
+++ b/src/robots/b2w_description/urdf/b2w_description.urdf
--- a/src/robots/b2w_description/xacro/const.xacro
+++ b/src/robots/b2w_description/xacro/const.xacro
@ -0,0 +1,160 @@
+<?xml version="1.0"?>
+
+<robot name="b2w_description" xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <!-- Constants for robot dimensions -->
+    <xacro:property name="PI" value="3.1415926535897931"/>
+    <!-- <xacro:property name="stick_mass" value="0.00001"/> -->
+
+    <!-- simplified collision value -->
+    <xacro:property name="trunk_width" value="0.28"/>
+    <xacro:property name="trunk_length" value="0.5"/>
+    <xacro:property name="trunk_height" value="0.15"/>
+    <xacro:property name="hip_radius" value="0.07"/>
+    <xacro:property name="hip_length" value="0.05"/>
+    <!-- <xacro:property name="thigh_shoulder_radius" value="0.044"/>
+    <xacro:property name="thigh_shoulder_length" value="0.08"/> -->
+    <xacro:property name="thigh_width" value="0.0455"/>
+    <xacro:property name="thigh_height" value="0.054"/>
+    <!-- <xacro:property name="calf_width" value="0.04"/>
+    <xacro:property name="calf_height" value="0.05"/> -->
+    <xacro:property name="foot_radius" value="0.04"/>
+    <!-- <xacro:property name="stick_radius" value="0.01"/>
+    <xacro:property name="stick_length" value="0.2"/> -->
+
+    <!-- kinematic value -->
+    <xacro:property name="thigh_offset" value="0.11973"/>
+    <xacro:property name="thigh_length" value="0.32"/>
+    <xacro:property name="calf_length" value="0.35"/>
+
+    <!-- leg offset from trunk center value -->
+    <xacro:property name="leg_offset_x" value="0.3285"/>
+    <xacro:property name="leg_offset_y" value="0.072"/>
+    <!-- <xacro:property name="trunk_offset_z" value="0.0"/> -->
+    <xacro:property name="hip_offset" value="0.11973"/>
+
+    <!-- offset of link and rotor locations (left front) -->
+    <!-- <xacro:property name="hip_offset_x" value="0.3285"/>
+    <xacro:property name="hip_offset_y" value="0.072"/>
+    <xacro:property name="hip_offset_z" value="0.0"/> -->
+    <xacro:property name="hip_rotor_offset_x" value="0.20205"/>
+    <xacro:property name="hip_rotor_offset_y" value="0.072"/>
+    <xacro:property name="hip_rotor_offset_z" value="0.0"/>
+
+    <!-- <xacro:property name="thigh_offset_x" value="0"/>
+    <xacro:property name="thigh_offset_y" value="0.11973"/>
+    <xacro:property name="thigh_offset_z" value="0.0"/> -->
+    <xacro:property name="thigh_rotor_offset_x" value="0.0"/>
+    <xacro:property name="thigh_rotor_offset_y" value="0.00798"/>
+    <xacro:property name="thigh_rotor_offset_z" value="0.0"/>
+
+    <!-- <xacro:property name="calf_offset_x" value="0.0"/>
+    <xacro:property name="calf_offset_y" value="0.0"/>
+    <xacro:property name="calf_offset_z" value="-0.35"/> -->
+    <xacro:property name="calf_rotor_offset_x" value="0.0"/>
+    <xacro:property name="calf_rotor_offset_y" value="-0.05788"/>
+    <xacro:property name="calf_rotor_offset_z" value="0.0"/>
+
+
+    <!-- joint limits -->
+    <xacro:property name="damping" value="0"/>
+    <xacro:property name="friction" value="0"/>
+    <xacro:property name="hip_position_max" value="0.87"/>
+    <xacro:property name="hip_position_min" value="-0.87"/>
+    <xacro:property name="hip_velocity_max" value="23"/>
+    <xacro:property name="hip_torque_max" value="200"/>
+    <xacro:property name="thigh_position_max" value="4.69"/>
+    <xacro:property name="thigh_position_min" value="-94"/>
+    <xacro:property name="thigh_velocity_max" value="23"/>
+    <xacro:property name="thigh_torque_max" value="200"/>
+    <xacro:property name="calf_position_max" value="-0.43"/>
+    <xacro:property name="calf_position_min" value="-2.82"/>
+    <xacro:property name="calf_velocity_max" value="14"/>
+    <xacro:property name="calf_torque_max" value="320"/>
+
+    <!-- dynamics inertial value -->
+    <!-- trunk -->
+    <xacro:property name="trunk_mass" value="35.606"/>
+    <xacro:property name="trunk_com_x" value="0.000458"/>
+    <xacro:property name="trunk_com_y" value="0.005261"/>
+    <xacro:property name="trunk_com_z" value="0.000665"/>
+    <xacro:property name="trunk_ixx" value="0.27466"/>
+    <xacro:property name="trunk_ixy" value="-0.000622"/>
+    <xacro:property name="trunk_ixz" value="-0.00315"/>
+    <xacro:property name="trunk_iyy" value="1.0618"/>
+    <xacro:property name="trunk_iyz" value="-0.00139"/>
+    <xacro:property name="trunk_izz" value="1.1825"/>
+
+    <!-- hip (left front) -->
+    <xacro:property name="hip_mass" value="2.256"/>
+    <xacro:property name="hip_com_x" value="-0.009305"/>
+    <xacro:property name="hip_com_y" value="-0.010228"/>
+    <xacro:property name="hip_com_z" value="0.000264"/>
+    <xacro:property name="hip_ixx" value="0.0026431"/>
+    <xacro:property name="hip_ixy" value="0.00019234"/>
+    <xacro:property name="hip_ixz" value="-6.76E-06"/>
+    <xacro:property name="hip_iyy" value="0.0046728"/>
+    <xacro:property name="hip_iyz" value="-7.16E-06"/>
+    <xacro:property name="hip_izz" value="0.0034208"/>
+
+    <xacro:property name="hip_rotor_mass" value="0.2734"/>
+    <xacro:property name="hip_rotor_com_x" value="0.0"/>
+    <xacro:property name="hip_rotor_com_y" value="0.0"/>
+    <xacro:property name="hip_rotor_com_z" value="0.0"/>
+    <xacro:property name="hip_rotor_ixx" value="0.000144463"/>
+    <xacro:property name="hip_rotor_ixy" value="0.0"/>
+    <xacro:property name="hip_rotor_ixz" value="0.0"/>
+    <xacro:property name="hip_rotor_iyy" value="0.000144463"/>
+    <xacro:property name="hip_rotor_iyz" value="0.0"/>
+    <xacro:property name="hip_rotor_izz" value="0.000263053"/>
+
+    <!-- thigh -->
+    <xacro:property name="thigh_mass" value="3.591"/>
+    <xacro:property name="thigh_com_x" value="-0.004346"/>
+    <xacro:property name="thigh_com_y" value="-0.035797"/>
+    <xacro:property name="thigh_com_z" value="-0.044921"/>
+    <xacro:property name="thigh_ixx" value="0.041718"/>
+    <xacro:property name="thigh_ixy" value="0.00055623"/>
+    <xacro:property name="thigh_ixz" value="-0.0022768"/>
+    <xacro:property name="thigh_iyy" value="0.041071"/>
+    <xacro:property name="thigh_iyz" value="0.005796"/>
+    <xacro:property name="thigh_izz" value="0.0065677"/>
+
+    <xacro:property name="thigh_rotor_mass" value="0.2734"/>
+    <xacro:property name="thigh_rotor_com_x" value="0.0"/>
+    <xacro:property name="thigh_rotor_com_y" value="0.0"/>
+    <xacro:property name="thigh_rotor_com_z" value="0.0"/>
+    <xacro:property name="thigh_rotor_ixx" value="0.000144463"/>
+    <xacro:property name="thigh_rotor_ixy" value="0.0"/>
+    <xacro:property name="thigh_rotor_ixz" value="0.0"/>
+    <xacro:property name="thigh_rotor_iyy" value="0.000144463"/>
+    <xacro:property name="thigh_rotor_iyz" value="0.0"/>
+    <xacro:property name="thigh_rotor_izz" value="0.000263053"/>
+
+    <!-- calf -->
+    <xacro:property name="calf_mass" value="2.053"/>
+    <xacro:property name="calf_com_x" value="0.006095"/>
+    <xacro:property name="calf_com_y" value="0.012046"/>
+    <xacro:property name="calf_com_z" value="-0.29122"/>
+    <xacro:property name="calf_ixx" value="0.028133"/>
+    <xacro:property name="calf_ixy" value="0.00016307"/>
+    <xacro:property name="calf_ixz" value="-0.0010034"/>
+    <xacro:property name="calf_iyy" value="0.029251"/>
+    <xacro:property name="calf_iyz" value="0.0014765"/>
+    <xacro:property name="calf_izz" value="0.0028379"/>
+
+    <xacro:property name="calf_rotor_mass" value="0.2734"/>
+    <xacro:property name="calf_rotor_com_x" value="0.0"/>
+    <xacro:property name="calf_rotor_com_y" value="0.0"/>
+    <xacro:property name="calf_rotor_com_z" value="0.0"/>
+    <xacro:property name="calf_rotor_ixx" value="0.000144463"/>
+    <xacro:property name="calf_rotor_ixy" value="0.0"/>
+    <xacro:property name="calf_rotor_ixz" value="0.0"/>
+    <xacro:property name="calf_rotor_iyy" value="0.000144463"/>
+    <xacro:property name="calf_rotor_iyz" value="0.0"/>
+    <xacro:property name="calf_rotor_izz" value="0.000263053"/>
+
+    <!-- foot -->
+    <xacro:property name="foot_mass" value="0.918"/>
+
+</robot>
--- a/src/robots/b2w_description/xacro/gazebo.xacro
+++ b/src/robots/b2w_description/xacro/gazebo.xacro
@ -0,0 +1,266 @@
+<?xml version="1.0"?>
+<robot>
+    <!-- ros_control plugin -->
+    <gazebo>
+        <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
+            <robotNamespace>/b2w_gazebo</robotNamespace>
+            <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+        </plugin>
+    </gazebo>
+
+    <!-- Show the trajectory of trunk center. -->
+    <gazebo>
+        <plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
+            <frequency>10</frequency>
+            <plot>
+                <link>base</link>
+                <pose>0 0 0 0 0 0</pose>
+                <material>Gazebo/Yellow</material>
+            </plot>
+        </plugin>
+    </gazebo>
+
+    <!-- Show the trajectory of foot. You can add another trajectory about another foot. -->
+    <!-- <gazebo>
+        <plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
+            <frequency>100</frequency>
+            <plot>
+                <link>FL_foot</link>
+                <pose>0 0 0 0 0 0</pose>
+                <material>Gazebo/Green</material>
+            </plot>
+        </plugin>
+    </gazebo> -->
+
+    <gazebo>
+        <plugin filename="libgazebo_ros_force.so" name="gazebo_ros_force">
+            <bodyName>trunk</bodyName>
+            <topicName>/apply_force/trunk</topicName>
+        </plugin>
+    </gazebo>
+
+    <gazebo reference="imu_link">
+        <gravity>true</gravity>
+        <sensor name="imu_sensor" type="imu">
+            <always_on>true</always_on>
+            <update_rate>1000</update_rate>
+            <visualize>true</visualize>
+            <topic>__default_topic__</topic>
+            <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
+                <topicName>trunk_imu</topicName>
+                <bodyName>imu_link</bodyName>
+                <updateRateHZ>1000.0</updateRateHZ>
+                <gaussianNoise>0.0</gaussianNoise>
+                <xyzOffset>0 0 0</xyzOffset>
+                <rpyOffset>0 0 0</rpyOffset>
+                <frameName>imu_link</frameName>
+            </plugin>
+            <pose>0 0 0 0 0 0</pose>
+        </sensor>
+    </gazebo>
+
+    <!-- Foot contacts. -->
+    <gazebo reference="FR_calf">
+        <sensor name="FR_foot_contact" type="contact">
+        <update_rate>100</update_rate>
+        <plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
+        <contact>
+            <collision>FR_calf_fixed_joint_lump__FR_foot_collision_1</collision>
+        </contact>
+        </sensor>
+    </gazebo>
+    <gazebo reference="FL_calf">
+        <sensor name="FL_foot_contact" type="contact">
+        <update_rate>100</update_rate>
+        <plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
+        <contact>
+            <collision>FL_calf_fixed_joint_lump__FL_foot_collision_1</collision>
+        </contact>
+        </sensor>
+    </gazebo>
+    <gazebo reference="RR_calf">
+        <sensor name="RR_foot_contact" type="contact">
+        <update_rate>100</update_rate>
+        <plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
+        <contact>
+            <collision>RR_calf_fixed_joint_lump__RR_foot_collision_1</collision>
+        </contact>
+        </sensor>
+    </gazebo>
+    <gazebo reference="RL_calf">
+        <sensor name="RL_foot_contact" type="contact">
+        <update_rate>100</update_rate>
+        <plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
+        <contact>
+            <collision>RL_calf_fixed_joint_lump__RL_foot_collision_1</collision>
+        </contact>
+        </sensor>
+    </gazebo>
+
+    <!-- Visualization of Foot contacts. -->
+    <!-- <gazebo reference="FR_foot">
+        <visual>
+            <plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
+                <topicName>FR_foot_contact</topicName>
+            </plugin>
+        </visual>
+    </gazebo>
+    <gazebo reference="FL_foot">
+        <visual>
+            <plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
+                <topicName>FL_foot_contact</topicName>
+            </plugin>
+        </visual>
+    </gazebo>
+    <gazebo reference="RR_foot">
+        <visual>
+            <plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
+                <topicName>RR_foot_contact</topicName>
+            </plugin>
+        </visual>
+    </gazebo>
+    <gazebo reference="RL_foot">
+        <visual>
+            <plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
+                <topicName>RL_foot_contact</topicName>
+            </plugin>
+        </visual>
+    </gazebo> -->
+
+    <gazebo reference="base">
+        <material>Gazebo/Green</material>
+        <turnGravityOff>false</turnGravityOff>
+    </gazebo>
+
+    <gazebo reference="trunk">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+
+    <gazebo reference="stick_link">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <material>Gazebo/White</material>
+    </gazebo>
+
+    <gazebo reference="imu_link">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <material>Gazebo/Red</material>
+    </gazebo>
+
+    <!-- FL leg -->
+    <gazebo reference="FL_hip">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <!-- <material>Gazebo/DarkGrey</material> -->
+    </gazebo>
+    <gazebo reference="FL_thigh">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <self_collide>1</self_collide>
+        <!-- <material>Gazebo/DarkGrey</material> -->
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+    <gazebo reference="FL_calf">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <self_collide>1</self_collide>
+    </gazebo>
+    <gazebo reference="FL_foot">
+        <mu1>0.6</mu1>
+        <mu2>0.6</mu2>
+        <self_collide>1</self_collide>
+        <!-- <material>Gazebo/DarkGrey</material> -->
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+
+    <!-- FR leg -->
+    <gazebo reference="FR_hip">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <!-- <material>Gazebo/DarkGrey</material> -->
+    </gazebo>
+    <gazebo reference="FR_thigh">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <self_collide>1</self_collide>
+        <!-- <material>Gazebo/DarkGrey</material> -->
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+    <gazebo reference="FR_calf">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <self_collide>1</self_collide>
+    </gazebo>
+    <gazebo reference="FR_foot">
+        <mu1>0.6</mu1>
+        <mu2>0.6</mu2>
+        <self_collide>1</self_collide>
+        <!-- <material>Gazebo/DarkGrey</material> -->
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+
+    <!-- RL leg -->
+    <gazebo reference="RL_hip">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <!-- <material>Gazebo/DarkGrey</material> -->
+    </gazebo>
+    <gazebo reference="RL_thigh">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <self_collide>1</self_collide>
+        <!-- <material>Gazebo/DarkGrey</material> -->
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+    <gazebo reference="RL_calf">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <self_collide>1</self_collide>
+    </gazebo>
+    <gazebo reference="RL_foot">
+        <mu1>0.6</mu1>
+        <mu2>0.6</mu2>
+        <self_collide>1</self_collide>
+        <!-- <material>Gazebo/DarkGrey</material> -->
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+
+    <!-- RR leg -->
+    <gazebo reference="RR_hip">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <!-- <material>Gazebo/DarkGrey</material> -->
+    </gazebo>
+    <gazebo reference="RR_thigh">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <self_collide>1</self_collide>
+        <!-- <material>Gazebo/DarkGrey</material> -->
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+    <gazebo reference="RR_calf">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <self_collide>1</self_collide>
+    </gazebo>
+    <gazebo reference="RR_foot">
+        <mu1>0.6</mu1>
+        <mu2>0.6</mu2>
+        <self_collide>1</self_collide>
+        <!-- <material>Gazebo/DarkGrey</material> -->
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+
+</robot>
--- a/src/robots/b2w_description/xacro/leg.xacro
+++ b/src/robots/b2w_description/xacro/leg.xacro
@ -0,0 +1,284 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <xacro:include filename="$(find b2w_description)/xacro/transmission.xacro"/>
+
+    <xacro:macro name="leg" params="name mirror mirror_dae front_hind front_hind_dae">
+
+        <joint name="${name}_hip_joint" type="revolute">
+            <xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
+                <origin rpy="0 0 0" xyz="${leg_offset_x} ${-leg_offset_y} 0"/>
+            </xacro:if>
+            <xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
+                <origin rpy="0 0 0" xyz="${leg_offset_x} ${leg_offset_y} 0"/>
+            </xacro:if>
+            <xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
+                <origin rpy="0 0 0" xyz="${-leg_offset_x} ${-leg_offset_y} 0"/>
+            </xacro:if>
+            <xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
+                <origin rpy="0 0 0" xyz="${-leg_offset_x} ${leg_offset_y} 0"/>
+            </xacro:if>
+            <parent link="trunk"/>
+            <child link="${name}_hip"/>
+            <axis xyz="1 0 0"/>
+            <dynamics damping="${damping}" friction="${friction}"/>
+            <xacro:if value="${(mirror_dae == True)}">
+                <limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${hip_position_min}" upper="${hip_position_max}"/>
+            </xacro:if>
+            <xacro:if value="${(mirror_dae == False)}">
+                <limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${-hip_position_max}" upper="${-hip_position_min}"/>
+            </xacro:if>
+        </joint>
+
+        <joint name="${name}_hip_rotor_joint" type="fixed">
+            <xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
+                <origin rpy="0 0 0" xyz="${hip_rotor_offset_x} ${-hip_rotor_offset_y} 0"/>
+            </xacro:if>
+            <xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
+                <origin rpy="0 0 0" xyz="${hip_rotor_offset_x} ${hip_rotor_offset_y} 0"/>
+            </xacro:if>
+            <xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
+                <origin rpy="0 0 0" xyz="${-hip_rotor_offset_x} ${-hip_rotor_offset_y} 0"/>
+            </xacro:if>
+            <xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
+                <origin rpy="0 0 0" xyz="${-hip_rotor_offset_x} ${hip_rotor_offset_y} 0"/>
+            </xacro:if>
+            <parent link="trunk"/>
+            <child link="${name}_hip_rotor"/>
+        </joint>
+
+        <link name="${name}_hip">
+            <visual>
+                <xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
+                    <origin rpy="0 0 0" xyz="0 0 0"/>
+                </xacro:if>
+                <xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
+                    <origin rpy="${PI} 0 0" xyz="0 0 0"/>
+                </xacro:if>
+                <xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
+                    <origin rpy="0 ${PI} 0" xyz="0 0 0"/>
+                </xacro:if>
+                <xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
+                    <origin rpy="${PI} ${PI} 0" xyz="0 0 0"/>
+                </xacro:if>
+                <geometry>
+                    <mesh filename="package://b2w_description/meshes/hip.dae" scale="1 1 1"/>
+                </geometry>
+                <material name="orange"/>
+            </visual>
+            <collision>
+                <origin rpy="${PI/2.0} 0 0" xyz="0 ${hip_offset*mirror} 0"/>
+                <geometry>
+                    <cylinder length="${hip_length}" radius="${hip_radius}"/>
+                </geometry>
+            </collision>
+            <inertial>
+                <origin rpy="0 0 0" xyz="${hip_com_x*front_hind} ${hip_com_y*mirror} ${hip_com_z}"/>
+                <mass value="${hip_mass}"/>
+                <inertia
+                    ixx="${hip_ixx}" ixy="${hip_ixy*mirror*front_hind}" ixz="${hip_ixz*front_hind}"
+                    iyy="${hip_iyy}" iyz="${hip_iyz*mirror}"
+                    izz="${hip_izz}"/>
+            </inertial>
+        </link>
+
+        <link name="${name}_hip_rotor">
+            <visual>
+                <origin rpy="0 ${PI/2.0} 0" xyz="0 0 0"/>
+                <geometry>
+                    <cylinder length="0.02" radius="0.05"/>
+                </geometry>
+                <material name="green"/>
+            </visual>
+            <inertial>
+                <origin rpy="0 0 0" xyz="${hip_rotor_com_x} ${hip_rotor_com_y} ${hip_rotor_com_z}"/>
+                <mass value="${hip_rotor_mass}"/>
+                <inertia
+                    ixx="${hip_rotor_ixx}" ixy="${hip_rotor_ixy}" ixz="${hip_rotor_ixz}"
+                    iyy="${hip_rotor_iyy}" iyz="${hip_rotor_iyz}"
+                    izz="${hip_rotor_izz}"/>
+            </inertial>
+        </link>
+
+        <joint name="${name}_thigh_joint" type="revolute">
+            <origin rpy="0 0 0" xyz="0 ${thigh_offset*mirror} 0"/>
+            <parent link="${name}_hip"/>
+            <child link="${name}_thigh"/>
+            <axis xyz="0 1 0"/>
+            <dynamics damping="${damping}" friction="${friction}"/>
+            <limit effort="${thigh_torque_max}" velocity="${thigh_velocity_max}" lower="${thigh_position_min}" upper="${thigh_position_max}"/>
+        </joint>
+
+        <joint name="${name}_thigh_rotor_joint" type="fixed">
+            <origin rpy="0 0 0" xyz="${thigh_rotor_offset_x} ${thigh_rotor_offset_y*mirror} 0"/>
+            <parent link="${name}_hip"/>
+            <child link="${name}_thigh_rotor"/>
+        </joint>
+
+        <link name="${name}_thigh">
+            <visual>
+                <origin rpy="0 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <xacro:if value="${mirror_dae == True}">
+                        <mesh filename="package://b2w_description/meshes/thigh.dae" scale="1 1 1"/>
+                    </xacro:if>
+                    <xacro:if value="${mirror_dae == False}">
+                        <mesh filename="package://b2w_description/meshes/thigh_mirror.dae" scale="1 1 1"/>
+                    </xacro:if>
+                </geometry>
+                <material name="orange"/>
+            </visual>
+            <collision>
+                <origin rpy="0 ${PI/2.0} 0" xyz="-0.025 0 ${-thigh_length/2.0}"/>
+                <geometry>
+                    <box size="${thigh_length} ${thigh_width} ${thigh_height}"/>
+                </geometry>
+            </collision>
+            <inertial>
+                <origin rpy="0 0 0" xyz="${thigh_com_x} ${thigh_com_y*mirror} ${thigh_com_z}"/>
+                <mass value="${thigh_mass}"/>
+                <inertia
+                    ixx="${thigh_ixx}" ixy="${thigh_ixy*mirror}" ixz="${thigh_ixz}"
+                    iyy="${thigh_iyy}" iyz="${thigh_iyz*mirror}"
+                    izz="${thigh_izz}"/>
+            </inertial>
+        </link>
+
+        <link name="${name}_thigh_rotor">
+            <visual>
+                <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <cylinder length="0.02" radius="0.05"/>
+                </geometry>
+                <material name="green"/>
+            </visual>
+            <inertial>
+                <origin rpy="0 0 0" xyz="${thigh_rotor_com_x} ${thigh_rotor_com_y} ${thigh_rotor_com_z}"/>
+                <mass value="${thigh_rotor_mass}"/>
+                <inertia
+                    ixx="${thigh_rotor_ixx}" ixy="${thigh_rotor_ixy}" ixz="${thigh_rotor_ixz}"
+                    iyy="${thigh_rotor_iyy}" iyz="${thigh_rotor_iyz}"
+                    izz="${thigh_rotor_izz}"/>
+            </inertial>
+        </link>
+
+        <joint name="${name}_calf_joint" type="revolute">
+            <origin rpy="0 0 0" xyz="0 0 ${-thigh_length}"/>
+            <parent link="${name}_thigh"/>
+            <child link="${name}_calf"/>
+            <axis xyz="0 1 0"/>
+            <dynamics damping="${damping}" friction="${friction}"/>
+            <limit effort="${calf_torque_max}" velocity="${calf_velocity_max}" lower="${calf_position_min}" upper="${calf_position_max}"/>
+        </joint>
+
+        <joint name="${name}_calf_rotor_joint" type="fixed">
+            <origin rpy="0 0 0" xyz="${calf_rotor_offset_x} ${calf_rotor_offset_y*mirror} 0"/>
+            <parent link="${name}_thigh"/>
+            <child link="${name}_calf_rotor"/>
+        </joint>
+
+        <link name="${name}_calf">
+            <visual>
+                <origin rpy="0 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <xacro:if value="${mirror_dae == True}">
+                        <mesh filename="package://b2w_description/meshes/thigh.dae" scale="1 1 1"/>
+                    </xacro:if>
+                    <xacro:if value="${mirror_dae == False}">
+                        <mesh filename="package://b2w_description/meshes/thigh_mirror.dae" scale="1 1 1"/>
+                    </xacro:if>
+                </geometry>
+                <material name="orange"/>
+            </visual>
+            <visual>
+                <origin rpy="0 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <xacro:if value="${mirror_dae == True}">
+                        <mesh filename="package://b2w_description/meshes/calf.dae" scale="1 1 1"/>
+                    </xacro:if>
+                    <xacro:if value="${mirror_dae == False}">
+                        <mesh filename="package://b2w_description/meshes/calf_mirror.dae" scale="1 1 1"/>
+                    </xacro:if>
+                </geometry>
+                <material name="orange"/>
+            </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>
+            <inertial>
+                <origin rpy="0 0 0" xyz="${calf_com_x} ${calf_com_y} ${calf_com_z}"/>
+                <mass value="${calf_mass}"/>
+                <inertia
+                    ixx="${calf_ixx}" ixy="${calf_ixy}" ixz="${calf_ixz}"
+                    iyy="${calf_iyy}" iyz="${calf_iyz}"
+                    izz="${calf_izz}"/>
+            </inertial>
+        </link>
+
+        <link name="${name}_calf_rotor">
+            <visual>
+                <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <cylinder length="0.02" radius="0.05"/>
+                </geometry>
+                <material name="green"/>
+            </visual>
+            <inertial>
+                <origin rpy="0 0 0" xyz="${calf_rotor_com_x} ${calf_rotor_com_y} ${calf_rotor_com_z}"/>
+                <mass value="${calf_rotor_mass}"/>
+                <inertia
+                    ixx="${calf_rotor_ixx}" ixy="${calf_rotor_ixy}" ixz="${calf_rotor_ixz}"
+                    iyy="${calf_rotor_iyy}" iyz="${calf_rotor_iyz}"
+                    izz="${calf_rotor_izz}"/>
+            </inertial>
+        </link>
+
+        <joint name="${name}_foot_fixed" type="fixed">
+            <origin rpy="1.57 0 0" xyz="0 0 ${-(calf_length)}"/>
+            <parent link="${name}_calf"/>
+            <child link="${name}_foot"/>
+        </joint>
+
+        <link name="${name}_foot">
+            <collision>
+            <origin
+                xyz="0 0 0"
+                rpy="0 0 0" />
+            <geometry>
+                <sphere radius="0.032" />
+            </geometry>
+            </collision>
+            <inertial>
+                <mass value="${foot_mass}"/>
+                <inertia
+                    ixx="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" ixy="0.0" ixz="0.0"
+                    iyy="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" iyz="0.0"
+                    izz="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}"/>
+            </inertial>
+        </link>
+
+        <xacro:leg_transmission name="${name}"/>
+    </xacro:macro>
+</robot>
--- a/src/robots/b2w_description/xacro/materials.xacro
+++ b/src/robots/b2w_description/xacro/materials.xacro
@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<robot>
+
+    <material name="black">
+        <color rgba="0.0 0.0 0.0 1.0"/>
+    </material>
+
+    <material name="blue">
+        <color rgba="0.0 0.0 0.8 1.0"/>
+    </material>
+
+    <material name="green">
+        <color rgba="0.0 0.8 0.0 1.0"/>
+    </material>
+
+    <material name="grey">
+        <color rgba="0.2 0.2 0.2 1.0"/>
+    </material>
+
+    <material name="silver">
+        <color rgba="${233/255} ${233/255} ${216/255} 1.0"/>
+    </material>
+
+    <material name="orange">
+        <color rgba="${255/255} ${108/255} ${10/255} 1.0"/>
+    </material>
+
+    <material name="brown">
+        <color rgba="${222/255} ${207/255} ${195/255} 1.0"/>
+    </material>
+
+    <material name="red">
+        <color rgba="0.8 0.0 0.0 1.0"/>
+    </material>
+
+    <material name="white">
+        <color rgba="1.0 1.0 1.0 1.0"/>
+    </material>
+
+</robot>
--- a/src/robots/b2w_description/xacro/robot.xacro
+++ b/src/robots/b2w_description/xacro/robot.xacro
@ -0,0 +1,182 @@
+<?xml version="1.0"?>
+
+<robot name="b2w_description" xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <xacro:include filename="$(find b2w_description)/xacro/const.xacro"/>
+    <xacro:include filename="$(find b2w_description)/xacro/materials.xacro"/>
+    <xacro:include filename="$(find b2w_description)/xacro/leg.xacro"/>
+    <xacro:include filename="$(find b2w_description)/xacro/gazebo.xacro"/>
+
+    <!-- Rotor related joint and link is only for demonstrate location. -->
+    <!-- Actually, the rotor will rotate and the joint is not fixed. Reduction ratio should be considered. -->
+
+    <!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
+    <xacro:if value="$(arg DEBUG)">
+        <link name="world"/>
+        <joint name="base_static_joint" type="fixed">
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <parent link="world"/>
+            <child link="base"/>
+        </joint>
+    </xacro:if>
+
+    <link name="base">
+        <visual>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <geometry>
+                <box size="0.001 0.001 0.001"/>
+            </geometry>
+        </visual>
+    </link>
+
+    <joint name="floating_base" type="fixed">
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <parent link="base"/>
+        <child link="trunk"/>
+    </joint>
+
+    <link name="trunk">
+        <visual>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <geometry>
+                <mesh filename="package://b2w_description/meshes/trunk.dae" scale="1 1 1"/>
+            </geometry>
+            <!-- <material name="orange"/> -->
+        </visual>
+        <collision>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <geometry>
+                <box size="${trunk_length} ${trunk_width} ${trunk_height}"/>
+            </geometry>
+        </collision>
+        <!-- <collision>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <geometry>
+                <mesh filename="package://b1_description/meshes/trunk.dae" scale="1 1 1"/>
+            </geometry>
+        </collision> -->
+        <inertial>
+            <origin rpy="0 0 0" xyz="${trunk_com_x} ${trunk_com_y} ${trunk_com_z}"/>
+            <mass value="${trunk_mass}"/>
+            <inertia
+                ixx="${trunk_ixx}" ixy="${trunk_ixy}" ixz="${trunk_ixz}"
+                iyy="${trunk_iyy}" iyz="${trunk_iyz}"
+                izz="${trunk_izz}"/>
+        </inertial>
+    </link>
+
+    <joint name="imu_joint" type="fixed">
+        <parent link="trunk"/>
+        <child link="imu_link"/>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+    </joint>
+
+    <link name="imu_link">
+        <inertial>
+            <mass value="0.001"/>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
+        </inertial>
+        <visual>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <geometry>
+                <box size="0.001 0.001 0.001"/>
+            </geometry>
+            <!-- <material name="red"/> -->
+        </visual>
+        <collision>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <geometry>
+                <box size=".001 .001 .001"/>
+            </geometry>
+        </collision>
+    </link>
+
+    <joint name="head_joint" type="fixed">
+        <parent link="trunk"/>
+        <child link="head_Link"/>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+    </joint>
+
+    <link name="head_Link">
+        <inertial>
+            <origin rpy="0 0 0" xyz="0.33989 -0.000168 0.12029"/>
+            <mass value="3.366" />
+            <inertia
+            ixx="0.026455"
+            ixy="3.15E-05"
+            ixz="0.00166"
+            iyy="0.029221"
+            iyz="-3.27E-05"
+            izz="0.010918" />
+        </inertial>
+        <collision>
+            <origin rpy="0 0 0" xyz="0.41 0 0.005"/>
+            <geometry>
+                <box size="0.04 0.12 0.14" />
+            </geometry>
+        </collision>
+    </link>
+
+    <joint name="lidar_joint" type="fixed">
+        <parent link="trunk"/>
+        <child link="lidar_link"/>
+        <origin rpy="0 0 0" xyz="0.34218 0 0.17851"/>
+    </joint>
+
+    <link name="lidar_link">
+        <inertial>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <mass value="6.55E-08" />
+            <inertia
+            ixx="1.64E-15"
+            ixy="0"
+            ixz="-1.09E-47"
+            iyy="1.64E-15"
+            iyz="-1.32E-47"
+            izz="1.64E-15" />
+        </inertial>
+        <collision>
+            <origin rpy="0 0 0" xyz="0 0 -0.02"/>
+            <geometry>
+                <cylinder length="0.16" radius="0.076" />
+            </geometry>
+        </collision>
+    </link>
+
+    <joint name="tail_joint" type="fixed">
+        <parent link="trunk"/>
+        <child link="tail_link"/>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+    </joint>
+
+    <link name="tail_link">
+        <inertial>
+            <origin rpy="0 0 0" xyz="-0.37448 0.000488 0.006495"/>
+            <mass value="0.777" />
+            <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 rpy="0 0 0" xyz="-0.405 0 0.005"/>
+            <geometry>
+                <box size="0.025 0.12 0.14" />
+            </geometry>
+        </collision>
+    </link>
+
+
+
+    <xacro:leg name="FR" mirror="-1" mirror_dae= "False" front_hind="1"  front_hind_dae="True"  />
+    <xacro:leg name="FL" mirror="1"  mirror_dae="True"   front_hind="1"  front_hind_dae="True"  />
+    <xacro:leg name="RR" mirror="-1" mirror_dae="False"  front_hind="-1" front_hind_dae="False" />
+    <xacro:leg name="RL" mirror="1"  mirror_dae="True"   front_hind="-1" front_hind_dae="False" />
+
+    <!-- <xacro:stairs stairs="15" xpos="0" ypos="2.0" zpos="0" /> -->
+
+</robot>
--- a/src/robots/b2w_description/xacro/stairs.xacro
+++ b/src/robots/b2w_description/xacro/stairs.xacro
@ -0,0 +1,46 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <xacro:property name="stair_length" value="0.640" />
+    <xacro:property name="stair_width" value="0.310" />
+    <xacro:property name="stair_height" value="0.170" />
+
+    <xacro:macro name="stairs" params="stairs xpos ypos zpos">
+
+        <joint name="stair_joint_origin" type="fixed">
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <parent link="world"/>
+            <child link="stair_link_${stairs}"/>
+        </joint>
+
+        <link name="stair_link_${stairs}">
+            <visual>
+                <geometry>
+                    <box size="${stair_length} ${stair_width} ${stair_height}"/>
+                </geometry>
+                <material name="grey"/>
+                <origin rpy="0 0 0" xyz="${xpos} ${ypos} ${zpos}"/>
+            </visual>
+            <collision>
+                <geometry>
+                    <box size="${stair_length} ${stair_width} ${stair_height}"/>
+                </geometry>
+            </collision>
+            <inertial>
+                <mass value="0.80"/>
+                <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+            </inertial>
+        </link>
+
+        <xacro:if value="${stairs}">
+            <xacro:stairs stairs="${stairs-1}" xpos="0" ypos="${ypos-stair_width/2}" zpos="${zpos+stair_height}"/>
+            <joint name="stair_joint_${stairs}" type="fixed">
+                <parent link="stair_link_${stairs}"/>
+                <child link="stair_link_${stairs-1}"/>
+            </joint>
+        </xacro:if>
+
+    </xacro:macro>
+
+</robot>
--- a/src/robots/b2w_description/xacro/transmission.xacro
+++ b/src/robots/b2w_description/xacro/transmission.xacro
@ -0,0 +1,42 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <xacro:macro name="leg_transmission" params="name">
+
+        <transmission name="${name}_hip_tran">
+            <type>transmission_interface/SimpleTransmission</type>
+            <joint name="${name}_hip_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            </joint>
+            <actuator name="${name}_hip_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+            </actuator>
+        </transmission>
+
+        <transmission name="${name}_thigh_tran">
+            <type>transmission_interface/SimpleTransmission</type>
+            <joint name="${name}_thigh_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            </joint>
+            <actuator name="${name}_thigh_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+            </actuator>
+        </transmission>
+
+        <transmission name="${name}_calf_tran">
+            <type>transmission_interface/SimpleTransmission</type>
+            <joint name="${name}_calf_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            </joint>
+            <actuator name="${name}_calf_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+            </actuator>
+        </transmission>
+
+    </xacro:macro>
+
+</robot>
--- a/src/robots/go2w_description/CMakeLists.txt
+++ b/src/robots/go2w_description/CMakeLists.txt
@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 2.8.3)
+
+project(go2w_description)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+find_package(roslaunch)
+
+foreach(dir config launch meshes urdf)
+	install(DIRECTORY ${dir}/
+		DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
+endforeach(dir)
--- a/src/robots/go2w_description/config/robot_control.yaml
+++ b/src/robots/go2w_description/config/robot_control.yaml
@ -0,0 +1,76 @@
+# Copyright (c) 2024-2025 Ziqi Fan
+# SPDX-License-Identifier: Apache-2.0
+
+go2w_gazebo:
+    # Publish all joint states -----------------------------------
+    joint_state_controller:
+        type: joint_state_controller/JointStateController
+        publish_rate: 1000
+
+    # FL Controllers ---------------------------------------
+    FL_hip_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: FL_hip_joint
+
+    FL_thigh_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: FL_thigh_joint
+
+    FL_calf_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: FL_calf_joint
+
+    FL_foot_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: FL_foot_joint
+
+    # FR Controllers ---------------------------------------
+    FR_hip_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: FR_hip_joint
+
+    FR_thigh_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: FR_thigh_joint
+
+    FR_calf_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: FR_calf_joint
+
+    FR_foot_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: FR_foot_joint
+
+    # RL Controllers ---------------------------------------
+    RL_hip_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: RL_hip_joint
+
+    RL_thigh_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: RL_thigh_joint
+
+    RL_calf_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: RL_calf_joint
+
+    RL_foot_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: RL_foot_joint
+
+    # RR Controllers ---------------------------------------
+    RR_hip_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: RR_hip_joint
+
+    RR_thigh_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: RR_thigh_joint
+
+    RR_calf_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: RR_calf_joint
+
+    RR_foot_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: RR_foot_joint
--- a/src/robots/go2w_description/dae/base.dae
+++ b/src/robots/go2w_description/dae/base.dae
--- a/src/robots/go2w_description/dae/calf.dae
+++ b/src/robots/go2w_description/dae/calf.dae
--- a/src/robots/go2w_description/dae/calf_mirror.dae
+++ b/src/robots/go2w_description/dae/calf_mirror.dae
--- a/src/robots/go2w_description/dae/foot.dae
+++ b/src/robots/go2w_description/dae/foot.dae
--- a/src/robots/go2w_description/dae/hip.dae
+++ b/src/robots/go2w_description/dae/hip.dae
--- a/src/robots/go2w_description/dae/left_wheel.dae
+++ b/src/robots/go2w_description/dae/left_wheel.dae
--- a/src/robots/go2w_description/dae/right_wheel.dae
+++ b/src/robots/go2w_description/dae/right_wheel.dae
--- a/src/robots/go2w_description/dae/thigh.dae
+++ b/src/robots/go2w_description/dae/thigh.dae
--- a/src/robots/go2w_description/dae/thigh_mirror.dae
+++ b/src/robots/go2w_description/dae/thigh_mirror.dae
--- a/src/robots/go2w_description/launch/check_joint.rviz
+++ b/src/robots/go2w_description/launch/check_joint.rviz
@ -0,0 +1,250 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /TF1/Frames1
+      Splitter Ratio: 0.5
+    Tree Height: 746
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 85; 87; 83
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        FL_calf:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FL_foot:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FL_hip:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_calf:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_foot:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_hip:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_thigh:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        Fl_thigh:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        Link Tree Style: Links in Alphabetic Order
+        RL_calf:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RL_foot:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RL_hip:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RL_thigh:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RR_calf:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RR_foot:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RR_hip:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RR_thigh:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        imu:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        radar:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/TF
+      Enabled: false
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+      Marker Alpha: 1
+      Marker Scale: 0.4000000059604645
+      Name: TF
+      Show Arrows: false
+      Show Axes: true
+      Show Names: true
+      Tree:
+        {}
+      Update Interval: 0
+      Value: false
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /link_inertias_viz
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 238; 238; 236
+    Default Light: true
+    Fixed Frame: base
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 0.7113326191902161
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: 0.14559818804264069
+        Y: 0.02902654930949211
+        Z: -0.1431877315044403
+      Focal Shape Fixed Size: false
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.5047972798347473
+      Target Frame: <Fixed Frame>
+      Yaw: 0.7604149580001831
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1043
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000015600000375fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000375000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000375fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000375000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000006240000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1920
+  X: 1920
+  Y: 0
--- a/src/robots/go2w_description/launch/gazebo.launch
+++ b/src/robots/go2w_description/launch/gazebo.launch
@ -0,0 +1,20 @@
+<launch>
+  <include
+    file="$(find gazebo_ros)/launch/empty_world.launch" />
+  <node
+    name="tf_footprint_base"
+    pkg="tf"
+    type="static_transform_publisher"
+    args="0 0 0 0 0 0 base_link base_footprint 40" />
+  <node
+    name="spawn_model"
+    pkg="gazebo_ros"
+    type="spawn_model"
+    args="-file $(find go2w_description)/urdf/go2w_description.urdf -urdf -model go2w_description"
+    output="screen" />
+  <node
+    name="fake_joint_calibration"
+    pkg="rostopic"
+    type="rostopic"
+    args="pub /calibrated std_msgs/Bool true" />
+</launch>
--- a/src/robots/go2w_description/launch/go2w_rviz.launch
+++ b/src/robots/go2w_description/launch/go2w_rviz.launch
@ -0,0 +1,22 @@
+<launch>
+
+    <arg name="user_debug" default="false"/>
+
+    <param name="robot_description"  textfile="$(find go2w_description)/urdf/go2w_description.urdf" />
+
+    <!-- for higher robot_state_publisher average rate-->
+    <!-- <param name="rate" value="1000"/> -->
+
+    <!-- send fake joint values -->
+    <node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui">
+        <param name="use_gui" value="TRUE"/>
+    </node>
+
+    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
+        <param name="publish_frequency" type="double" value="1000.0"/>
+    </node>
+
+    <node pkg="rviz" type="rviz" name="rviz" respawn="false" output="screen"
+        args="-d $(find go2w_description)/launch/check_joint.rviz"/>
+
+</launch>
--- a/src/robots/go2w_description/package.xml
+++ b/src/robots/go2w_description/package.xml
@ -0,0 +1,21 @@
+<package format="2">
+  <name>go2w_description</name>
+  <version>1.0.0</version>
+  <description>
+    <p>URDF Description package for go2w_description</p>
+    <p>This package contains configuration data, 3D models and launch files
+for go2w_description robot</p>
+  </description>
+  <author>TODO</author>
+  <maintainer email="TODO@email.com" />
+  <license>BSD</license>
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>roslaunch</depend>
+  <depend>robot_state_publisher</depend>
+  <depend>rviz</depend>
+  <depend>joint_state_publisher_gui</depend>
+  <depend>gazebo</depend>
+  <export>
+    <architecture_independent />
+  </export>
+</package>
--- a/src/robots/go2w_description/urdf/go2w_description.urdf
+++ b/src/robots/go2w_description/urdf/go2w_description.urdf