diff --git a/bioloid_controller/include/bioloid_controller_impl.h b/bioloid_controller/include/bioloid_controller_impl.h
index b068c35fe087ed9384e85417adb4f6554d461ad9..cd86c8ea186fcca8829458dd64103274a1bc2a59 100644
--- a/bioloid_controller/include/bioloid_controller_impl.h
+++ b/bioloid_controller/include/bioloid_controller_impl.h
@@ -479,7 +479,6 @@ namespace bioloid_controller
       else if(lr_imu_value<-300.0)
         lr_imu_value=-300.0;
       gyro_lr_value=((lr_imu_value*512)/300.0)+512;
-      std::cout << "fb_gyro: " << gyro_fb_value << ", lr_gyro: " << gyro_lr_value << std::endl;
     }
 
   template <class HardwareInterface>
diff --git a/bioloid_description/meshes/bounding_boxes/battery_pack2_bb.stl b/bioloid_description/meshes/bounding_boxes/battery_pack2_bb.stl
new file mode 100644
index 0000000000000000000000000000000000000000..9f9c63b725b78005ccc646fd784168dcff0d3707
Binary files /dev/null and b/bioloid_description/meshes/bounding_boxes/battery_pack2_bb.stl differ
diff --git a/bioloid_description/meshes/bounding_boxes/body_bb.stl b/bioloid_description/meshes/bounding_boxes/body_bb.stl
new file mode 100644
index 0000000000000000000000000000000000000000..423fbb9f382bbe88a1b1552e204c6f070dfa0c88
Binary files /dev/null and b/bioloid_description/meshes/bounding_boxes/body_bb.stl differ
diff --git a/bioloid_description/meshes/bounding_boxes/controller_bb.stl b/bioloid_description/meshes/bounding_boxes/controller_bb.stl
new file mode 100644
index 0000000000000000000000000000000000000000..ab6e3e91685435241559f39c1888f39f11cdcdd9
Binary files /dev/null and b/bioloid_description/meshes/bounding_boxes/controller_bb.stl differ
diff --git a/bioloid_description/meshes/bounding_boxes/left_arm_high_bb.stl b/bioloid_description/meshes/bounding_boxes/left_arm_high_bb.stl
new file mode 100644
index 0000000000000000000000000000000000000000..545f8db766c8f6085e485c09758f5405159a67ae
Binary files /dev/null and b/bioloid_description/meshes/bounding_boxes/left_arm_high_bb.stl differ
diff --git a/bioloid_description/meshes/bounding_boxes/left_arm_low_bb.stl b/bioloid_description/meshes/bounding_boxes/left_arm_low_bb.stl
new file mode 100644
index 0000000000000000000000000000000000000000..ceccf10a79a43bfeb3527b43f37a9ef6eba4c9e1
Binary files /dev/null and b/bioloid_description/meshes/bounding_boxes/left_arm_low_bb.stl differ
diff --git a/bioloid_description/meshes/bounding_boxes/left_leg_ankle_pitch_bb.stl b/bioloid_description/meshes/bounding_boxes/left_leg_ankle_pitch_bb.stl
new file mode 100644
index 0000000000000000000000000000000000000000..bb7f1805ba877fb88259763d8185106e74cfe763
Binary files /dev/null and b/bioloid_description/meshes/bounding_boxes/left_leg_ankle_pitch_bb.stl differ
diff --git a/bioloid_description/meshes/bounding_boxes/left_leg_ankle_roll_bb.stl b/bioloid_description/meshes/bounding_boxes/left_leg_ankle_roll_bb.stl
new file mode 100644
index 0000000000000000000000000000000000000000..6d84e522ec963ed0490fc4af3ea38b71f4c17ece
Binary files /dev/null and b/bioloid_description/meshes/bounding_boxes/left_leg_ankle_roll_bb.stl differ
diff --git a/bioloid_description/meshes/bounding_boxes/left_leg_knee_bb.stl b/bioloid_description/meshes/bounding_boxes/left_leg_knee_bb.stl
new file mode 100644
index 0000000000000000000000000000000000000000..7a6a9eb83b40e8a90d6fe1fbe4ee50200a31f2f7
Binary files /dev/null and b/bioloid_description/meshes/bounding_boxes/left_leg_knee_bb.stl differ
diff --git a/bioloid_description/meshes/bounding_boxes/left_leg_pelvis_pitch_bb.stl b/bioloid_description/meshes/bounding_boxes/left_leg_pelvis_pitch_bb.stl
new file mode 100644
index 0000000000000000000000000000000000000000..bcc7230875bff12dfeea6bb9f418ec33e27d7252
Binary files /dev/null and b/bioloid_description/meshes/bounding_boxes/left_leg_pelvis_pitch_bb.stl differ
diff --git a/bioloid_description/meshes/bounding_boxes/left_leg_pelvis_roll_bb.stl b/bioloid_description/meshes/bounding_boxes/left_leg_pelvis_roll_bb.stl
new file mode 100644
index 0000000000000000000000000000000000000000..166f26cd0d7b2509bd5530182c0012924379aefd
Binary files /dev/null and b/bioloid_description/meshes/bounding_boxes/left_leg_pelvis_roll_bb.stl differ
diff --git a/bioloid_description/meshes/bounding_boxes/left_leg_pelvis_yaw_bb.stl b/bioloid_description/meshes/bounding_boxes/left_leg_pelvis_yaw_bb.stl
new file mode 100644
index 0000000000000000000000000000000000000000..48027968de419b47cba7df403f2ab9d2a3aea8b2
Binary files /dev/null and b/bioloid_description/meshes/bounding_boxes/left_leg_pelvis_yaw_bb.stl differ
diff --git a/bioloid_description/meshes/bounding_boxes/left_shoulder_bb.stl b/bioloid_description/meshes/bounding_boxes/left_shoulder_bb.stl
new file mode 100644
index 0000000000000000000000000000000000000000..2b5d68a777111a901aae055b7f22167bfb04e6a3
Binary files /dev/null and b/bioloid_description/meshes/bounding_boxes/left_shoulder_bb.stl differ
diff --git a/bioloid_description/meshes/bounding_boxes/right_arm_high_bb.stl b/bioloid_description/meshes/bounding_boxes/right_arm_high_bb.stl
new file mode 100644
index 0000000000000000000000000000000000000000..7413bebbd670357f6fd0a03c0c22af4f347d1375
Binary files /dev/null and b/bioloid_description/meshes/bounding_boxes/right_arm_high_bb.stl differ
diff --git a/bioloid_description/meshes/bounding_boxes/right_arm_low_bb.stl b/bioloid_description/meshes/bounding_boxes/right_arm_low_bb.stl
new file mode 100644
index 0000000000000000000000000000000000000000..6a0d6ce53312ee5fa2813af0690bd96cdbc763e5
Binary files /dev/null and b/bioloid_description/meshes/bounding_boxes/right_arm_low_bb.stl differ
diff --git a/bioloid_description/meshes/bounding_boxes/right_leg_ankle_pitch_bb.stl b/bioloid_description/meshes/bounding_boxes/right_leg_ankle_pitch_bb.stl
new file mode 100644
index 0000000000000000000000000000000000000000..3e2f165c9b4e537db308b701b20efae9dc8bea46
Binary files /dev/null and b/bioloid_description/meshes/bounding_boxes/right_leg_ankle_pitch_bb.stl differ
diff --git a/bioloid_description/meshes/bounding_boxes/right_leg_ankle_roll_bb.stl b/bioloid_description/meshes/bounding_boxes/right_leg_ankle_roll_bb.stl
new file mode 100644
index 0000000000000000000000000000000000000000..8a667a2350bb3240c8f43c1e5841ec6b736c109a
Binary files /dev/null and b/bioloid_description/meshes/bounding_boxes/right_leg_ankle_roll_bb.stl differ
diff --git a/bioloid_description/meshes/bounding_boxes/right_leg_knee_bb.stl b/bioloid_description/meshes/bounding_boxes/right_leg_knee_bb.stl
new file mode 100644
index 0000000000000000000000000000000000000000..e213abbb8de0d532e987c35ac1f00732b5003a00
Binary files /dev/null and b/bioloid_description/meshes/bounding_boxes/right_leg_knee_bb.stl differ
diff --git a/bioloid_description/meshes/bounding_boxes/right_leg_pelvis_pitch_bb.stl b/bioloid_description/meshes/bounding_boxes/right_leg_pelvis_pitch_bb.stl
new file mode 100644
index 0000000000000000000000000000000000000000..8557e85986763186b9e35a82f6329f668f20a46a
Binary files /dev/null and b/bioloid_description/meshes/bounding_boxes/right_leg_pelvis_pitch_bb.stl differ
diff --git a/bioloid_description/meshes/bounding_boxes/right_leg_pelvis_roll_bb.stl b/bioloid_description/meshes/bounding_boxes/right_leg_pelvis_roll_bb.stl
new file mode 100644
index 0000000000000000000000000000000000000000..bc8ed84a2119633b9c8682b4addb8e3319730e87
Binary files /dev/null and b/bioloid_description/meshes/bounding_boxes/right_leg_pelvis_roll_bb.stl differ
diff --git a/bioloid_description/meshes/bounding_boxes/right_leg_pelvis_yaw_bb.stl b/bioloid_description/meshes/bounding_boxes/right_leg_pelvis_yaw_bb.stl
new file mode 100644
index 0000000000000000000000000000000000000000..55fda47b22ccee8a842ae58174ec4001c88559e9
Binary files /dev/null and b/bioloid_description/meshes/bounding_boxes/right_leg_pelvis_yaw_bb.stl differ
diff --git a/bioloid_description/meshes/bounding_boxes/right_shoulder_bb.stl b/bioloid_description/meshes/bounding_boxes/right_shoulder_bb.stl
new file mode 100644
index 0000000000000000000000000000000000000000..944b79fd778668bbd785dbe011dac70255f232a6
Binary files /dev/null and b/bioloid_description/meshes/bounding_boxes/right_shoulder_bb.stl differ
diff --git a/bioloid_description/meshes/bounding_boxes/smart_battery_charger_board_bb.stl b/bioloid_description/meshes/bounding_boxes/smart_battery_charger_board_bb.stl
new file mode 100644
index 0000000000000000000000000000000000000000..acbe1add872de8f60ba023c738988cbd1dcd3dc2
Binary files /dev/null and b/bioloid_description/meshes/bounding_boxes/smart_battery_charger_board_bb.stl differ
diff --git a/bioloid_description/urdf/bioloid.xacro b/bioloid_description/urdf/bioloid.xacro
index a12cdb7283e83fc5556fb0912240d3d17fcc123d..fab09ee176446ea0cc6c6d6c7f0e234f02626770 100755
--- a/bioloid_description/urdf/bioloid.xacro
+++ b/bioloid_description/urdf/bioloid.xacro
@@ -3,6 +3,12 @@
   <xacro:include filename="$(find bioloid_description)/urdf/bioloid.gazebo" />
 
   <link name="base_link">
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/bounding_boxes/body_bb.stl" />
+      </geometry>
+    </collision>
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
@@ -20,6 +26,12 @@
   </link>
 
   <link name="right_shoulder">
+    <collision>
+      <origin xyz="0.0 -0.0145 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/bounding_boxes/right_shoulder_bb.stl" />
+      </geometry>
+    </collision>
     <visual>
       <origin xyz="0.0 -0.0145 0" rpy="0 0 0" />
       <geometry>
@@ -37,6 +49,12 @@
   </link>
 
   <link name="right_arm_high" >
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/bounding_boxes/right_arm_high_bb.stl" />
+      </geometry>
+    </collision>
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
@@ -54,6 +72,12 @@
   </link>
 
   <link name="right_arm_low" >
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/bounding_boxes/right_arm_low_bb.stl" />
+      </geometry>
+    </collision>
     <visual>
       <origin xyz="0.00 0.0 0.0" rpy="0 0 0" />
       <geometry>
@@ -71,6 +95,12 @@
   </link>
 
   <link name="left_shoulder">
+    <collision>
+      <origin xyz="0.0 -0.0145 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/bounding_boxes/left_shoulder_bb.stl" />
+      </geometry>
+    </collision>
     <visual>
       <origin xyz="0.0 -0.0145 0" rpy="0 0 0" />
       <geometry>
@@ -88,6 +118,12 @@
   </link>
 
   <link name="left_arm_high" >
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/bounding_boxes/left_arm_high_bb.stl" />
+      </geometry>
+    </collision>
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
@@ -105,6 +141,12 @@
   </link>
 
   <link name="left_arm_low" >
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/bounding_boxes/left_arm_low_bb.stl" />
+      </geometry>
+    </collision>
     <visual>
       <origin xyz="0.00 0.0 0.0" rpy="0 0 0" />
       <geometry>
@@ -122,6 +164,12 @@
   </link>
 
   <link name="right_leg_pelvis_yaw" >
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/bounding_boxes/right_leg_pelvis_yaw_bb.stl" />
+      </geometry>
+    </collision>
     <visual>
       <origin xyz="0 0.0 0" rpy="0 0 0" />
       <geometry>
@@ -139,6 +187,12 @@
   </link>
 
   <link name="right_leg_pelvis_roll" >
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/bounding_boxes/right_leg_pelvis_roll_bb.stl" />
+      </geometry>
+    </collision>
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
@@ -156,6 +210,12 @@
   </link>
 
   <link name="right_leg_pelvis_pitch" >
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/bounding_boxes/right_leg_pelvis_pitch_bb.stl" />
+      </geometry>
+    </collision>
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
@@ -173,6 +233,12 @@
   </link>
 
   <link name="right_leg_knee" >
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/bounding_boxes/right_leg_knee_bb.stl" />
+      </geometry>
+    </collision>
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
@@ -190,6 +256,12 @@
   </link>
 
   <link name="right_leg_ankle_pitch" >
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/bounding_boxes/right_leg_ankle_pitch_bb.stl" />
+      </geometry>
+    </collision>
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
@@ -208,9 +280,9 @@
 
   <link name="right_leg_ankle_roll" >
     <collision>
-      <origin xyz="0.0 -0.031 0.0" rpy="0 0 0"/>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
       <geometry>
-        <box size="0.060 0.002 0.1"/>
+        <mesh filename="package://bioloid_description/meshes/bounding_boxes/right_leg_ankle_roll_bb.stl" />
       </geometry>
     </collision>
     <visual>
@@ -230,6 +302,12 @@
   </link>
 
   <link name="left_leg_pelvis_yaw" >
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/bounding_boxes/left_leg_pelvis_yaw_bb.stl" />
+      </geometry>
+    </collision>
     <visual>
       <origin xyz="0 0.0 0" rpy="0 0 0" />
       <geometry>
@@ -247,6 +325,12 @@
   </link>
 
   <link name="left_leg_pelvis_roll" >
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/bounding_boxes/left_leg_pelvis_roll_bb.stl" />
+      </geometry>
+    </collision>
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
@@ -264,6 +348,12 @@
   </link>
 
   <link name="left_leg_pelvis_pitch" >
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/bounding_boxes/left_leg_pelvis_pitch_bb.stl" />
+      </geometry>
+    </collision>
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
@@ -281,6 +371,12 @@
   </link>
 
   <link name="left_leg_knee" >
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/bounding_boxes/left_leg_knee_bb.stl" />
+      </geometry>
+    </collision>
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
@@ -298,6 +394,12 @@
   </link>
 
   <link name="left_leg_ankle_pitch" >
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/bounding_boxes/left_leg_ankle_pitch_bb.stl" />
+      </geometry>
+    </collision>
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
@@ -315,6 +417,12 @@
   </link>
 
   <link name="left_leg_ankle_roll" >
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/bounding_boxes/left_leg_ankle_roll_bb.stl" />
+      </geometry>
+    </collision>
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
@@ -329,12 +437,6 @@
       <origin xyz="0.00000000 -0.02582749 -0.00471367" rpy="0 0 0"/>
       <inertia ixx="0.00003001" ixy="0.0" ixz="0.0" iyy="0.00003576" iyz="-0.00000137" izz="0.00000999" />
     </inertial>
-    <collision>
-      <origin xyz="0.0 -0.031 0.0" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.060 0.002 0.1"/>
-      </geometry>
-    </collision>
   </link>
 
   <joint name="j_shoulder_r" type="revolute">