diff --git a/bioloid_control/CMakeLists.txt b/bioloid_control/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..d79f5d5913ac00799fb7aaedcb337ed6a8b93b54
--- /dev/null
+++ b/bioloid_control/CMakeLists.txt
@@ -0,0 +1,156 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(bioloid_control)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependencies might have been
+##     pulled in transitively but can be declared for certainty nonetheless:
+##     * add a build_depend tag for "message_generation"
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES bioloid_control
+#  CATKIN_DEPENDS other_catkin_pkg
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+
+## Declare a cpp library
+# add_library(bioloid_control
+#   src/${PROJECT_NAME}/bioloid_control.cpp
+# )
+
+## Declare a cpp executable
+# add_executable(bioloid_control_node src/bioloid_control_node.cpp)
+
+## Add cmake target dependencies of the executable/library
+## as an example, message headers may need to be generated before nodes
+# add_dependencies(bioloid_control_node bioloid_control_generate_messages_cpp)
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(bioloid_control_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS bioloid_control bioloid_control_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_bioloid_control.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/bioloid_control/config/bioloid_control.yaml b/bioloid_control/config/bioloid_control.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..a20adb9f19604bec43f2a196a5bac596ea76011b
--- /dev/null
+++ b/bioloid_control/config/bioloid_control.yaml
@@ -0,0 +1,230 @@
+bioloid:
+  # Publish all joint states -----------------------------------
+  joint_state_controller:
+    type: joint_state_controller/JointStateController
+    publish_rate: 50  
+
+  bioloid_controller:
+    type: effort_controllers/JointTrajectoryController
+    joints:
+      - j_shoulder_l
+      - j_high_arm_l
+      - j_low_arm_l
+      - j_shoulder_r
+      - j_high_arm_r
+      - j_low_arm_r
+      - j_pelvis_yaw_l
+      - j_pelvis_roll_l
+      - j_pelvis_pitch_l
+      - j_knee_l
+      - j_ankle_pitch_l
+      - j_ankle_roll_l
+      - j_pelvis_yaw_r
+      - j_pelvis_roll_r
+      - j_pelvis_pitch_r
+      - j_knee_r
+      - j_ankle_pitch_r
+      - j_ankle_roll_r
+    gains:
+      j_shoulder_l:
+        p: 32.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 2.5
+      j_high_arm_l:
+        p: 32.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 2.5
+      j_low_arm_l:
+        p: 32.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 2.5
+      j_shoulder_r:
+        p: 32.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 2.5
+      j_high_arm_r:
+        p: 32.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 2.5
+      j_low_arm_r:
+        p: 32.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 2.5
+      j_pelvis_yaw_l:
+        p: 32.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 2.5
+      j_pelvis_roll_l:
+        p: 32.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 2.5
+      j_pelvis_pitch_l:
+        p: 32.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 2.5
+      j_knee_l:
+        p: 32.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 2.5
+      j_ankle_pitch_l:
+        p: 32.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 2.5
+      j_ankle_roll_l:
+        p: 32.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 2.5
+      j_pelvis_yaw_r:
+        p: 32.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 2.5
+      j_pelvis_roll_r:
+        p: 32.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 2.5
+      j_pelvis_pitch_r:
+        p: 32.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 2.5
+      j_knee_r:
+        p: 32.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 2.5
+      j_ankle_pitch_r:
+        p: 32.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 2.5
+      j_ankle_roll_r:
+        p: 32.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 2.5
+    joint_trajectory_action_node:
+      joints:
+        - j_shoulder_l
+        - j_high_arm_l
+        - j_low_arm_l
+        - j_shoulder_r
+        - j_high_arm_r
+        - j_low_arm_r
+        - j_pelvis_yaw_l
+        - j_pelvis_roll_l
+        - j_pelvis_pitch_l
+        - j_knee_l
+        - j_ankle_pitch_l
+        - j_ankle_roll_l
+        - j_pelvis_yaw_r
+        - j_pelvis_roll_r
+        - j_pelvis_pitch_r
+        - j_knee_r
+        - j_ankle_pitch_r
+        - j_ankle_roll_r
+      constraints:
+        goal_time: 1.0
+        j_shoulder_l:
+          goal: 0.0
+        j_high_arm_l:
+          goal: 0.0
+        j_low_arm_l:
+          goal: 0.0
+        j_shoulder_r:
+          goal: 0.0
+        j_high_arm_r:
+          goal: 0.0
+        j_low_arm_r:
+          goal: 0.0
+        j_pelvis_yaw_l:
+          goal: 0.0
+        j_pelvis_roll_l:
+          goal: 0.0
+        j_pelvis_pitch_l:
+          goal: 0.0
+        j_knee_l:
+          goal: 0.0
+        j_ankle_pitch_l:
+          goal: 0.0
+        j_ankle_roll_l:
+          goal: 0.0
+        j_pelvis_yaw_r:
+          goal: 0.0
+        j_pelvis_roll_r:
+          goal: 0.0
+        j_pelvis_pitch_r:
+          goal: 0.0
+        j_knee_r:
+          goal: 0.0
+        j_ankle_pitch_r:
+          goal: 0.0
+        j_ankle_roll_r:
+          goal: 0.0
diff --git a/bioloid_control/launch/bioloid_control.launch b/bioloid_control/launch/bioloid_control.launch
new file mode 100644
index 0000000000000000000000000000000000000000..2df76786b088f011826f4efb758772c7ba7cee8c
--- /dev/null
+++ b/bioloid_control/launch/bioloid_control.launch
@@ -0,0 +1,11 @@
+<launch>
+
+  <!-- Load joint controller configurations from YAML file to parameter server -->
+  <rosparam file="$(find bioloid_control)/config/bioloid_control.yaml" command="load"/>
+
+  <!-- load the controllers -->
+  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+    output="screen" ns="/bioloid" args="bioloid_controller joint_state_controller"/>
+
+</launch>
+
diff --git a/bioloid_control/package.xml b/bioloid_control/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..306f01a6c701f3f9658a653ee7e9306794f2beb1
--- /dev/null
+++ b/bioloid_control/package.xml
@@ -0,0 +1,54 @@
+<?xml version="1.0"?>
+<package>
+  <name>bioloid_control</name>
+  <version>0.0.0</version>
+  <description>The bioloid_control package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="sergi@todo.todo">sergi</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/bioloid_control</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <run_depend>controller_manager</run_depend>
+  <run_depend>robot_mechanism_controllers</run_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- You can specify that this package is a metapackage here: -->
+    <!-- <metapackage/> -->
+
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
diff --git a/bioloid_description/config/bioloid_base.rviz b/bioloid_description/config/bioloid_base.rviz
new file mode 100644
index 0000000000000000000000000000000000000000..863a1ca797386d9a2df41814e499a76f317dbca9
--- /dev/null
+++ b/bioloid_description/config/bioloid_base.rviz
@@ -0,0 +1,221 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1
+      Splitter Ratio: 0.5
+    Tree Height: 775
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        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
+        Link Tree Style: Links in Alphabetic Order
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_arm_high:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_arm_low:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_leg_ankle_pitch:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_leg_ankle_roll:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_leg_knee:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_leg_pelvis_pitch:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_leg_pelvis_roll:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_leg_pelvis_yaw:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_shoulder:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_arm_high:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_arm_low:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_leg_ankle_pitch:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_leg_ankle_roll:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_leg_knee:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_leg_pelvis_pitch:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_leg_pelvis_roll:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_leg_pelvis_yaw:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_shoulder:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: /bioloid
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: bioloid/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
+      Topic: /initialpose
+    - 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.659061
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0.101358
+        Y: -0.119063
+        Z: 0.0267737
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 1.4098
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 3.45859
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1056
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000013c00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000004e80000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1855
+  X: 58
+  Y: 24
diff --git a/bioloid_description/launch/bioloid_base.launch b/bioloid_description/launch/bioloid_base.launch
index 6e26f754afa23d0a59ef7979eebbd8f8702bdcd2..3c3c5ad68b69f753830ce284c2ab16b4f67c8835 100644
--- a/bioloid_description/launch/bioloid_base.launch
+++ b/bioloid_description/launch/bioloid_base.launch
@@ -7,10 +7,10 @@
     <param name="tf_prefix" type="string" value="/bioloid"/>
     <param name="publish_frequency" type="double" value="20.0"/>
     <remap from="/joint_states" to="/bioloid/joint_states" />
-  </node>
+  </node>>
 
   <node name="rviz" 
         pkg="rviz" 
         type="rviz" 
-        args="-d $(find darwin_description)/config/bioloid_rviz.rviz" />
+        args="-d $(find bioloid_description)/config/bioloid_base.rviz" />
 </launch>
diff --git a/bioloid_description/launch/bioloid_test.launch b/bioloid_description/launch/bioloid_test.launch
new file mode 100644
index 0000000000000000000000000000000000000000..9f18a50428aad9a6d160f829dacfb947beba1d1e
--- /dev/null
+++ b/bioloid_description/launch/bioloid_test.launch
@@ -0,0 +1,20 @@
+<launch>
+  <!-- Convert an xacro and put on parameter server -->
+  <param name="robot_description"
+         command="$(find xacro)/xacro.py '$(find bioloid_description)/urdf/bioloid.xacro'" />
+
+  <node pkg="robot_state_publisher" type="state_publisher" name="bioloid_tf_broadcaster">
+    <param name="tf_prefix" type="string" value="/bioloid"/>
+    <param name="publish_frequency" type="double" value="20.0"/>
+<!--    <remap from="/joint_states" to="/bioloid/joint_states" />-->
+  </node>>
+
+  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
+    <param name="use_gui" value="true"/>
+  </node>
+
+  <node name="rviz" 
+        pkg="rviz" 
+        type="rviz" 
+        args="-d $(find bioloid_description)/config/bioloid_base.rviz" />
+</launch>
diff --git a/bioloid_description/meshes/left_arm_high.stl b/bioloid_description/meshes/left_arm_high.stl
new file mode 100755
index 0000000000000000000000000000000000000000..fccc48600c1401d191803c60f5beaa77c3eb5b92
Binary files /dev/null and b/bioloid_description/meshes/left_arm_high.stl differ
diff --git a/bioloid_description/meshes/left_arm_low.stl b/bioloid_description/meshes/left_arm_low.stl
new file mode 100755
index 0000000000000000000000000000000000000000..5ba1a49552e5304a55b0ef925ad7404c102920ca
Binary files /dev/null and b/bioloid_description/meshes/left_arm_low.stl differ
diff --git a/bioloid_description/meshes/left_leg_ankle_pitch.stl b/bioloid_description/meshes/left_leg_ankle_pitch.stl
new file mode 100755
index 0000000000000000000000000000000000000000..8e293219f78c7538988a0c4dca81fec0e4a58e8d
Binary files /dev/null and b/bioloid_description/meshes/left_leg_ankle_pitch.stl differ
diff --git a/bioloid_description/meshes/left_leg_ankle_roll.stl b/bioloid_description/meshes/left_leg_ankle_roll.stl
new file mode 100755
index 0000000000000000000000000000000000000000..daea5b456994994be073bcdfb001ed068dcc838a
Binary files /dev/null and b/bioloid_description/meshes/left_leg_ankle_roll.stl differ
diff --git a/bioloid_description/meshes/left_leg_knee.stl b/bioloid_description/meshes/left_leg_knee.stl
new file mode 100755
index 0000000000000000000000000000000000000000..06eb2e1df087bbd9cbb62973c2a61058589de034
Binary files /dev/null and b/bioloid_description/meshes/left_leg_knee.stl differ
diff --git a/bioloid_description/meshes/left_leg_pelvis_pitch.stl b/bioloid_description/meshes/left_leg_pelvis_pitch.stl
new file mode 100755
index 0000000000000000000000000000000000000000..1e265619cb81a0fffd63b1bb1baa446909206636
Binary files /dev/null and b/bioloid_description/meshes/left_leg_pelvis_pitch.stl differ
diff --git a/bioloid_description/meshes/left_leg_pelvis_roll.stl b/bioloid_description/meshes/left_leg_pelvis_roll.stl
new file mode 100755
index 0000000000000000000000000000000000000000..a1521257f77b8f06c350f995089d19204f94bb2a
Binary files /dev/null and b/bioloid_description/meshes/left_leg_pelvis_roll.stl differ
diff --git a/bioloid_description/meshes/left_leg_pelvis_yaw.stl b/bioloid_description/meshes/left_leg_pelvis_yaw.stl
new file mode 100755
index 0000000000000000000000000000000000000000..98c8cc88c4a58c9daaceb779b1991af248e346fd
Binary files /dev/null and b/bioloid_description/meshes/left_leg_pelvis_yaw.stl differ
diff --git a/bioloid_description/meshes/left_shoulder.stl b/bioloid_description/meshes/left_shoulder.stl
new file mode 100755
index 0000000000000000000000000000000000000000..fa8e5aee8486e0d1565a63c6bf0e3084c8a2a03b
Binary files /dev/null and b/bioloid_description/meshes/left_shoulder.stl differ
diff --git a/bioloid_description/meshes/right_arm_high.stl b/bioloid_description/meshes/right_arm_high.stl
new file mode 100755
index 0000000000000000000000000000000000000000..a0c3a66c7509999e6d9dce753536c4b2d6b291de
Binary files /dev/null and b/bioloid_description/meshes/right_arm_high.stl differ
diff --git a/bioloid_description/meshes/right_arm_low.stl b/bioloid_description/meshes/right_arm_low.stl
new file mode 100755
index 0000000000000000000000000000000000000000..7feec44599e3de44ad5b0c40cc75db505c35d0e3
Binary files /dev/null and b/bioloid_description/meshes/right_arm_low.stl differ
diff --git a/bioloid_description/meshes/right_leg_ankle_pitch.stl b/bioloid_description/meshes/right_leg_ankle_pitch.stl
new file mode 100755
index 0000000000000000000000000000000000000000..58a0978bb124675082051cf082a8a1c5580bc336
Binary files /dev/null and b/bioloid_description/meshes/right_leg_ankle_pitch.stl differ
diff --git a/bioloid_description/meshes/right_leg_ankle_roll.stl b/bioloid_description/meshes/right_leg_ankle_roll.stl
new file mode 100755
index 0000000000000000000000000000000000000000..a4ba09e0c7e02c287a168a4b2d3238a15d359390
Binary files /dev/null and b/bioloid_description/meshes/right_leg_ankle_roll.stl differ
diff --git a/bioloid_description/meshes/right_leg_knee.stl b/bioloid_description/meshes/right_leg_knee.stl
new file mode 100755
index 0000000000000000000000000000000000000000..f544dcf8be3e1a63ba1ae1d413242ec942c570eb
Binary files /dev/null and b/bioloid_description/meshes/right_leg_knee.stl differ
diff --git a/bioloid_description/meshes/right_leg_pelvis_pitch.stl b/bioloid_description/meshes/right_leg_pelvis_pitch.stl
new file mode 100755
index 0000000000000000000000000000000000000000..9a641b8dd118dcf4eea49142b078a69e43aabd7d
Binary files /dev/null and b/bioloid_description/meshes/right_leg_pelvis_pitch.stl differ
diff --git a/bioloid_description/meshes/right_leg_pelvis_roll.stl b/bioloid_description/meshes/right_leg_pelvis_roll.stl
new file mode 100755
index 0000000000000000000000000000000000000000..e6277279ed507cc66615eaac98934b181bcb1957
Binary files /dev/null and b/bioloid_description/meshes/right_leg_pelvis_roll.stl differ
diff --git a/bioloid_description/meshes/right_leg_pelvis_yaw.stl b/bioloid_description/meshes/right_leg_pelvis_yaw.stl
new file mode 100755
index 0000000000000000000000000000000000000000..0c3f8c69cd3e258134d9308ea18e6eb937f0e70c
Binary files /dev/null and b/bioloid_description/meshes/right_leg_pelvis_yaw.stl differ
diff --git a/bioloid_description/meshes/right_shoulder.stl b/bioloid_description/meshes/right_shoulder.stl
new file mode 100755
index 0000000000000000000000000000000000000000..08b5ac690af3eb2fad17f38fbbc811116bbb8ca5
Binary files /dev/null and b/bioloid_description/meshes/right_shoulder.stl differ
diff --git a/bioloid_description/urdf/bioloid.gazebo b/bioloid_description/urdf/bioloid.gazebo
new file mode 100644
index 0000000000000000000000000000000000000000..83aee761f2faa3bfbb86f86cba20867f3ec82e38
--- /dev/null
+++ b/bioloid_description/urdf/bioloid.gazebo
@@ -0,0 +1,313 @@
+<?xml version="1.0"?>
+<robot>
+  <gazebo>
+    <static>0</static>
+  </gazebo>
+
+  <gazebo reference="base_link">
+    <material>Gazebo/Grey</material>
+  </gazebo>
+
+  <gazebo reference="left_shoulder">
+    <material>Gazebo/Grey</material>
+  </gazebo>
+
+  <gazebo reference="left_arm_high">
+    <material>Gazebo/Grey</material>
+  </gazebo>
+
+  <gazebo reference="left_arm_low">
+    <material>Gazebo/Grey</material>
+  </gazebo>
+
+  <gazebo reference="right_shoulder">
+    <material>Gazebo/Grey</material>
+  </gazebo>
+
+  <gazebo reference="right_arm_high">
+    <material>Gazebo/Grey</material>
+  </gazebo>
+
+  <gazebo reference="right_arm_low">
+    <material>Gazebo/Grey</material>
+  </gazebo>
+
+  <gazebo reference="left_leg_pelvis_yaw">
+    <material>Gazebo/Grey</material>
+  </gazebo>
+
+  <gazebo reference="left_leg_pelvis_roll">
+    <material>Gazebo/Grey</material>
+  </gazebo>
+
+  <gazebo reference="left_leg_pelvis_pitch">
+    <material>Gazebo/Grey</material>
+  </gazebo>
+
+  <gazebo reference="left_leg_knee">
+    <material>Gazebo/Grey</material>
+  </gazebo>
+
+  <gazebo reference="left_leg_ankle_pitch">
+    <material>Gazebo/Grey</material>
+  </gazebo>
+
+  <gazebo reference="left_leg_ankle_roll">
+    <material>Gazebo/Grey</material>
+  </gazebo>
+
+  <gazebo reference="right_leg_pelvis_yaw">
+    <material>Gazebo/Grey</material>
+  </gazebo>
+
+  <gazebo reference="right_leg_pelvis_roll">
+    <material>Gazebo/Grey</material>
+  </gazebo>
+
+  <gazebo reference="right_leg_pelvis_pitch">
+    <material>Gazebo/Grey</material>
+  </gazebo>
+
+  <gazebo reference="right_leg_knee">
+    <material>Gazebo/Grey</material>
+  </gazebo>
+
+  <gazebo reference="right_leg_ankle_pitch">
+    <material>Gazebo/Grey</material>
+  </gazebo>
+
+  <gazebo reference="right_leg_ankle_roll">
+    <material>Gazebo/Grey</material>
+  </gazebo>
+
+  <gazebo reference="base_link">
+    <gravity>true</gravity>
+    <self_collide>false</self_collide>
+    <dampingFactor>0.9</dampingFactor>
+    <mu1>1.000000</mu1>
+    <mu2>1.000000</mu2>
+  </gazebo>
+
+  <gazebo reference="left_shoulder">
+    <gravity>true</gravity>
+    <self_collide>false</self_collide>
+    <dampingFactor>0.9</dampingFactor>
+    <mu1>0.000000</mu1>
+    <mu2>0.000000</mu2>
+  </gazebo>
+
+  <gazebo reference="left_arm_high">
+    <gravity>true</gravity>
+    <self_collide>false</self_collide>
+    <dampingFactor>0.9</dampingFactor>
+    <mu1>0.000000</mu1>
+    <mu2>0.000000</mu2>
+  </gazebo>
+
+  <gazebo reference="left_arm_low">
+    <gravity>true</gravity>
+    <self_collide>false</self_collide>
+    <dampingFactor>0.9</dampingFactor>
+    <mu1>0.000000</mu1>
+    <mu2>0.000000</mu2>
+  </gazebo>
+
+  <gazebo reference="right_shoulder">
+    <gravity>true</gravity>
+    <self_collide>false</self_collide>
+    <dampingFactor>0.9</dampingFactor>
+    <mu1>0.000000</mu1>
+    <mu2>0.000000</mu2>
+  </gazebo>
+
+  <gazebo reference="right_arm_high">
+    <gravity>true</gravity>
+    <self_collide>false</self_collide>
+    <dampingFactor>0.9</dampingFactor>
+    <mu1>0.000000</mu1>
+    <mu2>0.000000</mu2>
+  </gazebo>
+
+  <gazebo reference="right_arm_low">
+    <gravity>true</gravity>
+    <self_collide>false</self_collide>
+    <dampingFactor>0.9</dampingFactor>
+    <mu1>0.000000</mu1>
+    <mu2>0.000000</mu2>
+  </gazebo>
+
+  <gazebo reference="left_leg_pelvis_yaw">
+    <gravity>true</gravity>
+    <self_collide>false</self_collide>
+    <dampingFactor>0.9</dampingFactor>
+    <mu1>0.000000</mu1>
+    <mu2>0.000000</mu2>
+  </gazebo>
+
+  <gazebo reference="left_leg_pelvis_roll">
+    <gravity>true</gravity>
+    <self_collide>false</self_collide>
+    <dampingFactor>0.9</dampingFactor>
+    <mu1>0.000000</mu1>
+    <mu2>0.000000</mu2>
+  </gazebo>
+
+  <gazebo reference="left_leg_pelvis_pitch">
+    <gravity>true</gravity>
+    <self_collide>false</self_collide>
+    <dampingFactor>0.9</dampingFactor>
+    <mu1>0.000000</mu1>
+    <mu2>0.000000</mu2>
+  </gazebo>
+
+  <gazebo reference="left_leg_knee">
+    <gravity>true</gravity>
+    <self_collide>false</self_collide>
+    <dampingFactor>0.9</dampingFactor>
+    <mu1>0.000000</mu1>
+    <mu2>0.000000</mu2>
+  </gazebo>
+
+  <gazebo reference="left_leg_ankle_pitch">
+    <gravity>true</gravity>
+    <self_collide>false</self_collide>
+    <dampingFactor>0.9</dampingFactor>
+    <mu1>0.000000</mu1>
+    <mu2>0.000000</mu2>
+  </gazebo>
+
+  <gazebo reference="left_leg_ankle_roll">
+    <gravity>true</gravity>
+    <self_collide>false</self_collide>
+    <dampingFactor>0.9</dampingFactor>
+    <mu1>0.000000</mu1>
+    <mu2>0.000000</mu2>
+  </gazebo>
+
+  <gazebo reference="right_leg_pelvis_yaw">
+    <gravity>true</gravity>
+    <self_collide>false</self_collide>
+    <dampingFactor>0.9</dampingFactor>
+    <mu1>0.000000</mu1>
+    <mu2>0.000000</mu2>
+  </gazebo>
+
+  <gazebo reference="right_leg_pelvis_roll">
+    <gravity>true</gravity>
+    <self_collide>false</self_collide>
+    <dampingFactor>0.9</dampingFactor>
+    <mu1>0.000000</mu1>
+    <mu2>0.000000</mu2>
+  </gazebo>
+
+  <gazebo reference="right_leg_pelvis_pitch">
+    <gravity>true</gravity>
+    <self_collide>false</self_collide>
+    <dampingFactor>0.9</dampingFactor>
+    <mu1>0.000000</mu1>
+    <mu2>0.000000</mu2>
+  </gazebo>
+
+  <gazebo reference="right_leg_knee">
+    <gravity>true</gravity>
+    <self_collide>false</self_collide>
+    <dampingFactor>0.9</dampingFactor>
+    <mu1>0.000000</mu1>
+    <mu2>0.000000</mu2>
+  </gazebo>
+
+  <gazebo reference="right_leg_ankle_pitch">
+    <gravity>true</gravity>
+    <self_collide>false</self_collide>
+    <dampingFactor>0.9</dampingFactor>
+    <mu1>0.000000</mu1>
+    <mu2>0.000000</mu2>
+  </gazebo>
+
+  <gazebo reference="right_leg_ankle_roll">
+    <gravity>true</gravity>
+    <self_collide>false</self_collide>
+    <dampingFactor>0.9</dampingFactor>
+    <mu1>0.000000</mu1>
+    <mu2>0.000000</mu2>
+  </gazebo>
+
+  <gazebo reference="j_shoulder_l">
+    <implicitSpringDamper>true</implicitSpringDamper>
+  </gazebo>
+
+  <gazebo reference="j_high_arm_l">
+    <implicitSpringDamper>true</implicitSpringDamper>
+  </gazebo>
+
+  <gazebo reference="j_low_arm_l">
+    <implicitSpringDamper>true</implicitSpringDamper>
+  </gazebo>
+
+  <gazebo reference="j_shoulder_r">
+    <implicitSpringDamper>true</implicitSpringDamper>
+  </gazebo>
+
+  <gazebo reference="j_high_arm_r">
+    <implicitSpringDamper>true</implicitSpringDamper>
+  </gazebo>
+
+  <gazebo reference="j_low_arm_r">
+    <implicitSpringDamper>true</implicitSpringDamper>
+  </gazebo>
+
+  <gazebo reference="j_pelvis_yaw_l">
+    <implicitSpringDamper>true</implicitSpringDamper>
+  </gazebo>
+
+  <gazebo reference="j_pelvis_roll_l">
+    <implicitSpringDamper>true</implicitSpringDamper>
+  </gazebo>
+
+  <gazebo reference="j_pelvis_pitch_l">
+    <implicitSpringDamper>true</implicitSpringDamper>
+  </gazebo>
+
+  <gazebo reference="j_knee_l">
+    <implicitSpringDamper>true</implicitSpringDamper>
+  </gazebo>
+
+  <gazebo reference="j_ankle_pitch_l">
+    <implicitSpringDamper>true</implicitSpringDamper>
+  </gazebo>
+
+  <gazebo reference="j_ankle_roll_l">
+    <implicitSpringDamper>true</implicitSpringDamper>
+  </gazebo>
+
+  <gazebo reference="j_pelvis_yaw_r">
+    <implicitSpringDamper>true</implicitSpringDamper>
+  </gazebo>
+
+  <gazebo reference="j_pelvis_roll_r">
+    <implicitSpringDamper>true</implicitSpringDamper>
+  </gazebo>
+
+  <gazebo reference="j_pelvis_pitch_r">
+    <implicitSpringDamper>true</implicitSpringDamper>
+  </gazebo>
+
+  <gazebo reference="j_knee_r">
+    <implicitSpringDamper>true</implicitSpringDamper>
+  </gazebo>
+
+  <gazebo reference="j_ankle_pitch_r">
+    <implicitSpringDamper>true</implicitSpringDamper>
+  </gazebo>
+
+  <gazebo reference="j_ankle_roll_r">
+    <implicitSpringDamper>true</implicitSpringDamper>
+  </gazebo>
+
+  <gazebo>
+    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
+      <robotNamespace>/bioloid</robotNamespace>
+    </plugin>
+  </gazebo>
+
+</robot>
diff --git a/bioloid_description/urdf/bioloid.xacro b/bioloid_description/urdf/bioloid.xacro
index 486ef74cc5348e57c0b935643fad82f9c387444c..ed8975cb3734b42bb0e96d01515400c6576adf84 100755
--- a/bioloid_description/urdf/bioloid.xacro
+++ b/bioloid_description/urdf/bioloid.xacro
@@ -1,16 +1,8 @@
 <robot name="bioloid" xmlns:xacro="http://www.ros.org/wiki/xacro">
  
-<!--  <xacro:include filename="$(find bioloid_description)/urdf/bioloid.gazebo" />-->
+  <xacro:include filename="$(find bioloid_description)/urdf/bioloid.gazebo" />
 
-<!--  <link name="world"/>
-
-  <joint name="j_world" type="fixed">
-    <parent link="world"/>
-    <child link="MP_BODY"/>
-    <origin xyz="0 0 0.3" rpy="1.5707 0 0" />
-  </joint>-->
-
-  <link name="MP_BODY">
+  <link name="base_link">
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
@@ -26,4 +18,647 @@
       <inertia ixx="0.00006808" ixy="0.00000000" ixz="0.00000029" iyy="0.00006762" iyz="-0.00000053" izz="0.00009639" />
     </inertial>
   </link>
+
+  <link name="left_shoulder">
+    <visual>
+      <origin xyz="0.0 -0.0145 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/left_shoulder.stl" />
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.79 0.82 0.93 1.0"/>
+      </material>
+    </visual>
+    <inertial>
+      <mass value="0.00795999" />
+      <origin xyz="0.01223178 0.01071957 0.0" rpy="0 0 0"/>
+      <inertia ixx="0.00000355" ixy="0.00000031" ixz="0.00000000" iyy="0.00000349" iyz="0.00000000" izz="0.00000147" />
+    </inertial>
+  </link>
+
+  <link name="left_arm_high" >
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/left_arm_high.stl" />
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.79 0.82 0.93 1.0"/>
+      </material>
+    </visual>
+    <inertial>
+      <mass value="0.06399809" />
+      <origin xyz="-0.00002662 -0.01859163 0.00083789" rpy="0 0 0"/>
+      <inertia ixx="0.00000410" ixy="0.00000000" ixz="0.00000000" iyy="0.00000353" iyz="0.00000000" izz="0.00000182" />
+    </inertial>
+  </link>
+
+  <link name="left_arm_low" >
+    <visual>
+      <origin xyz="0.00 0.0 0.0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/left_arm_low.stl" />
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.79 0.82 0.93 1.0"/>
+      </material>
+    </visual>
+    <inertial>
+      <mass value="0.06340277" />
+      <origin xyz="-0.00032576 -0.01820781 0.00084575" rpy="0 0 0"/>
+      <inertia ixx="0.00000200" ixy="0.00000033" ixz="0.00000000" iyy="0.00000169" iyz="0.00000000" izz="0.00000157" />
+    </inertial>
+  </link>
+
+  <link name="right_shoulder">
+    <visual>
+      <origin xyz="0.0 -0.0145 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/right_shoulder.stl" />
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.79 0.82 0.93 1.0"/>
+      </material>
+    </visual>
+    <inertial>
+      <mass value="0.00796031" />
+      <origin xyz="-0.01223141 0.01071920 0.00000000" rpy="0 0 0"/>
+      <inertia ixx="0.00000355" ixy="-0.00000031" ixz="0.00000000" iyy="0.00000349" iyz="0.00000000" izz="0.00000147" />
+    </inertial>
+  </link>
+
+  <link name="right_arm_high" >
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/right_arm_high.stl" />
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.79 0.82 0.93 1.0"/>
+      </material>
+    </visual>
+    <inertial>
+      <mass value="0.06399809" />
+      <origin xyz="-0.00002662 -0.01859163 0.00083789" rpy="0 0 0"/>
+      <inertia ixx="0.00000410" ixy="-0.000000" ixz="0.00000000" iyy="0.00000353" iyz="0.00000000" izz="0.00000182" />
+    </inertial>
+  </link>
+
+  <link name="right_arm_low" >
+    <visual>
+      <origin xyz="0.00 0.0 0.0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/right_arm_low.stl" />
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.79 0.82 0.93 1.0"/>
+      </material>
+    </visual>
+    <inertial>
+      <mass value="0.06340277" />
+      <origin xyz="0.00027383 -0.01820781 0.00084575" rpy="0 0 0"/>
+      <inertia ixx="0.00000200" ixy="-0.00000033" ixz="0.00000000" iyy="0.00000169" iyz="0.00000000" izz="0.00000157" />
+    </inertial>
+  </link>
+
+  <link name="left_leg_pelvis_yaw" >
+    <visual>
+      <origin xyz="0 0.0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/left_leg_pelvis_yaw.stl" />
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.79 0.82 0.93 1.0"/>
+      </material>
+    </visual>
+    <inertial>
+      <mass value="0.01083078" />
+      <origin xyz="-0.00000246 0.01893788 -0.01648262" rpy="0 0 0"/>
+      <inertia ixx="0.00001023" ixy="0.0" ixz="0.0" iyy="0.00000978" iyz="0.00000008" izz="0.00000205" />
+    </inertial>
+  </link>
+
+  <link name="left_leg_pelvis_roll" >
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/left_leg_pelvis_roll.stl" />
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.79 0.82 0.93 1.0"/>
+      </material>
+    </visual>
+    <inertial>
+      <mass value="0.11724768" />
+      <origin xyz="0.00047126 -0.01276672 -0.01614616" rpy="0 0 0"/>
+      <inertia ixx="0.00000754" ixy="0.0" ixz="0.0" iyy="0.00000897" iyz="0.00000047" izz="0.00000303" />
+    </inertial>
+  </link>
+
+  <link name="left_leg_pelvis_pitch" >
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/left_leg_pelvis_pitch.stl" />
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.79 0.82 0.93 1.0"/>
+      </material>
+    </visual>
+    <inertial>
+      <mass value="0.02111586" />
+      <origin xyz="0.00000000 -0.04341218 -0.00107102" rpy="0 0 0"/>
+      <inertia ixx="0.00001158" ixy="0.0" ixz="0.0" iyy="0.00000965" iyz="0.00000047" izz="0.00001750" />
+    </inertial>
+  </link>
+
+  <link name="left_leg_knee" >
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/left_leg_knee.stl" />
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.79 0.82 0.93 1.0"/>
+      </material>
+    </visual>
+    <inertial>
+      <mass value="0.07160661" />
+      <origin xyz="0.00074885 -0.00918420 0.01330283" rpy="0 0 0"/>
+      <inertia ixx="0.00000785" ixy="0.0" ixz="0.0" iyy="0.00000697" iyz="0.00000003" izz="0.00001221" />
+    </inertial>
+  </link>
+
+  <link name="left_leg_ankle_pitch" >
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/left_leg_ankle_pitch.stl" />
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.79 0.82 0.93 1.0"/>
+      </material>
+    </visual>
+    <inertial>
+      <mass value="0.11724768" />
+      <origin xyz="0.00044344 0.01276672 -0.01617397" rpy="0 0 0"/>
+      <inertia ixx="0.00000754" ixy="0.0" ixz="0.0" iyy="0.00000897" iyz="-0.00000047" izz="0.00000303" />
+    </inertial>
+  </link>
+
+  <link name="left_leg_ankle_roll" >
+    <collision>
+      <origin xyz="0.0 -0.031 0.0" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.060 0.002 0.1"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/left_leg_ankle_roll.stl" />
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.79 0.82 0.93 1.0"/>
+      </material>
+    </visual>
+    <inertial>
+      <mass value="0.03089600" />
+      <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>
+  </link>
+
+  <link name="right_leg_pelvis_yaw" >
+    <visual>
+      <origin xyz="0 0.0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/right_leg_pelvis_yaw.stl" />
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.79 0.82 0.93 1.0"/>
+      </material>
+    </visual>
+    <inertial>
+      <mass value="0.01083078" />
+      <origin xyz="-0.00000246 0.01893788 -0.01648262" rpy="0 0 0"/>
+      <inertia ixx="0.00001023" ixy="0.0" ixz="0.0" iyy="0.00000978" iyz="0.00000008" izz="0.00000205" />
+    </inertial>
+  </link>
+
+  <link name="right_leg_pelvis_roll" >
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/right_leg_pelvis_roll.stl" />
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.79 0.82 0.93 1.0"/>
+      </material>
+    </visual>
+    <inertial>
+      <mass value="0.11724768" />
+      <origin xyz="-0.00044344 -0.01276672 -0.01617397" rpy="0 0 0"/>
+      <inertia ixx="0.00000754" ixy="0.0" ixz="0.0" iyy="0.00000897" iyz="0.00000047" izz="0.00000303" />
+    </inertial>
+  </link>
+
+  <link name="right_leg_pelvis_pitch" >
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/right_leg_pelvis_pitch.stl" />
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.79 0.82 0.93 1.0"/>
+      </material>
+    </visual>
+    <inertial>
+      <mass value="0.02111586" />
+      <origin xyz="0.00000000 -0.04341217 -0.00107156" rpy="0 0 0"/>
+      <inertia ixx="0.00001158" ixy="0.0" ixz="0.0" iyy="0.00000965" iyz="0.00000090" izz="0.00001750" />
+    </inertial>
+  </link>
+
+  <link name="right_leg_knee" >
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/right_leg_knee.stl" />
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.79 0.82 0.93 1.0"/>
+      </material>
+    </visual>
+    <inertial>
+      <mass value="0.07160661" />
+      <origin xyz="-0.00074886 -0.00922974 0.01330148" rpy="0 0 0"/>
+      <inertia ixx="0.00000785" ixy="0.0" ixz="0.0" iyy="0.00000697" iyz="0.00000003" izz="0.00001221" />
+    </inertial>
+  </link>
+
+  <link name="right_leg_ankle_pitch" >
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/right_leg_ankle_pitch.stl" />
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.79 0.82 0.93 1.0"/>
+      </material>
+    </visual>
+    <inertial>
+      <mass value="0.11724768" />
+      <origin xyz="-0.00047126 0.01276672 -0.01614616" rpy="0 0 0"/>
+      <inertia ixx="0.00000754" ixy="0.0" ixz="0.0" iyy="0.00000897" iyz="-0.00000047" izz="0.00000303" />
+    </inertial>
+  </link>
+
+  <link name="right_leg_ankle_roll" >
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://bioloid_description/meshes/right_leg_ankle_roll.stl" />
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.79 0.82 0.93 1.0"/>
+      </material>
+    </visual>
+    <inertial>
+      <mass value="0.03089600" />
+      <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_l" type="revolute">
+    <parent link="base_link"/>
+    <child link="left_shoulder"/>
+    <origin xyz="-0.07188 0.0 0" rpy="0 0 0" />
+    <axis xyz="1 0 0" />
+    <limit effort="2.500" velocity="6.0" lower="-1.81" upper="1.81" />
+    <dynamics damping="0.3"/>
+  </joint>
+
+  <transmission name="tran_shoulder_l">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j_shoulder_l"/>
+    <actuator name="shoulder_l_motor">
+      <hardwareInterface>EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <joint name="j_high_arm_l" type="revolute">
+    <parent link="left_shoulder"/>
+    <child link="left_arm_high"/>
+    <origin xyz="0 -0.0145 0.0" rpy="0 0 0" />
+    <axis xyz="0 0 1" />
+    <limit effort="2.500" velocity="6.0" lower="-1.81" upper="1.81" />
+    <dynamics damping="0.3"/>
+  </joint>
+
+  <transmission name="tran_high_arm_l">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j_high_arm_l"/>
+    <actuator name="high_arm_l_motor">
+      <hardwareInterface>EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <joint name="j_low_arm_l" type="revolute">
+    <parent link="left_arm_high"/>
+    <child link="left_arm_low"/>
+    <origin xyz="0.0 -0.0675 0.0" rpy="0 0 0" />
+    <axis xyz="0 0 1" />
+    <limit effort="2.500" velocity="6.0" lower="-1.81" upper="1.81" />
+    <dynamics damping="0.3"/>
+  </joint>
+
+  <transmission name="tran_low_arm_l">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j_low_arm_l"/>
+    <actuator name="low_arm_l_motor">
+      <hardwareInterface>EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <joint name="j_shoulder_r" type="revolute">
+    <parent link="base_link"/>
+    <child link="right_shoulder"/>
+    <origin xyz="0.07188 0.0 0" rpy="0 0 0" />
+    <axis xyz="1 0 0" />
+    <limit effort="2.500" velocity="6.0" lower="-1.81" upper="1.81" />
+    <dynamics damping="0.3"/>
+  </joint>
+
+  <transmission name="tran_shoulder_r">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j_shoulder_r"/>
+    <actuator name="shoulder_r_motor">
+      <hardwareInterface>EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <joint name="j_high_arm_r" type="revolute">
+    <parent link="right_shoulder"/>
+    <child link="right_arm_high"/>
+    <origin xyz="0 -0.0145 0.0" rpy="0 0 0" />
+    <axis xyz="0 0 1" />
+    <limit effort="2.500" velocity="6.0" lower="-1.81" upper="1.81" />
+    <dynamics damping="0.3"/>
+  </joint>
+
+  <transmission name="tran_high_arm_r">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j_high_arm_r"/>
+    <actuator name="high_arm_r_motor">
+      <hardwareInterface>EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <joint name="j_low_arm_r" type="revolute">
+    <parent link="right_arm_high"/>
+    <child link="right_arm_low"/>
+    <origin xyz="0.0 -0.0675 0.0" rpy="0 0 0" />
+    <axis xyz="0 0 1" />
+    <limit effort="2.500" velocity="6.0" lower="-1.81" upper="1.81" />
+    <dynamics damping="0.3"/>
+  </joint>
+
+  <transmission name="tran_low_arm_r">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j_low_arm_r"/>
+    <actuator name="low_arm_r_motor">
+      <hardwareInterface>EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <joint name="j_pelvis_yaw_l" type="revolute">
+    <parent link="base_link"/>
+    <child link="left_leg_pelvis_yaw"/>
+    <origin xyz="-0.0385 -0.12037 0.0" rpy="0 0 0" />
+    <axis xyz="0 1 0" />
+    <limit effort="2.500" velocity="6.0" lower="-2.5" upper="0.69" />
+    <dynamics damping="0.3"/>
+  </joint>
+
+  <transmission name="tran_pelvis_yaw_l">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j_pelvis_yaw_l"/>
+    <actuator name="pelvis_yaw_l_motor">
+      <hardwareInterface>EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <joint name="j_pelvis_roll_l" type="revolute">
+    <parent link="left_leg_pelvis_yaw"/>
+    <child link="left_leg_pelvis_roll"/>
+    <origin xyz="0 0 0" rpy="0 0 0" />
+    <axis xyz="0 0 1" />
+    <limit effort="2.500" velocity="6.0" lower="-0.93" upper="1.0" />
+    <dynamics damping="0.3"/>
+  </joint>
+
+  <transmission name="tran_pelvis_roll_l">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j_pelvis_roll_l"/>
+    <actuator name="pelvis_roll_l_motor">
+      <hardwareInterface>EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <joint name="j_pelvis_pitch_l" type="revolute">
+    <parent link="left_leg_pelvis_roll"/>
+    <child link="left_leg_pelvis_pitch"/>
+    <origin xyz="0 0 0" rpy="0 0 0" />
+    <axis xyz="1 0 0" />
+    <limit effort="2.500" velocity="6.0" lower="-1.68" upper="0.5" />
+    <dynamics damping="0.3"/>
+  </joint>
+
+  <transmission name="tran_pelvis_roll_l">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j_pelvis_pitch_l"/>
+    <actuator name="pelvis_pitch_l_motor">
+      <hardwareInterface>EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <joint name="j_knee_l" type="revolute">
+    <parent link="left_leg_pelvis_pitch"/>
+    <child link="left_leg_knee"/>
+    <origin xyz="0 -0.075 -0.01488" rpy="0 0 0" />
+    <axis xyz="1 0 0" />
+    <limit effort="2.500" velocity="6.0" lower="-0.03" upper="2.25" />
+    <dynamics damping="0.3"/>
+  </joint>
+
+  <transmission name="tran_knee_l">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j_knee_l"/>
+    <actuator name="knee_l_motor">
+      <hardwareInterface>EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <joint name="j_ankle_pitch_l" type="revolute">
+    <parent link="left_leg_knee"/>
+    <child link="left_leg_ankle_pitch"/>
+    <origin xyz="0 -0.075 0.01488" rpy="0 0 0" />
+    <axis xyz="1 0 0" />
+    <limit effort="2.500" velocity="6.0" lower="-1.22" upper="1.39" />
+    <dynamics damping="0.3"/>
+  </joint>
+
+  <transmission name="tran_ankle_pitch_l">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j_ankle_pitch_l"/>
+    <actuator name="ankle_pitch_l_motor">
+      <hardwareInterface>EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <joint name="j_ankle_roll_l" type="revolute">
+    <parent link="left_leg_ankle_pitch"/>
+    <child link="left_leg_ankle_roll"/>
+    <origin xyz="0 0 0" rpy="0 0 0" />
+    <axis xyz="0 0 1" />
+    <limit effort="2.500" velocity="6.0" lower="-1.02" upper="0.6" />
+    <dynamics damping="0.3"/>
+  </joint>
+
+  <transmission name="tran_ankle_roll_l">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j_ankle_roll_l"/>
+    <actuator name="ankle_roll_l_motor">
+      <hardwareInterface>EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <joint name="j_pelvis_yaw_r" type="revolute">
+    <parent link="base_link"/>
+    <child link="right_leg_pelvis_yaw"/>
+    <origin xyz="0.0385 -0.12037 0.0" rpy="0 0 0" />
+    <axis xyz="0 1 0" />
+    <limit effort="2.500" velocity="6.0" lower="-2.5" upper="0.69" />
+    <dynamics damping="0.3"/>
+  </joint>
+
+  <transmission name="tran_pelvis_yaw_r">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j_pelvis_yaw_r"/>
+    <actuator name="pelvis_yaw_r_motor">
+      <hardwareInterface>EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <joint name="j_pelvis_roll_r" type="revolute">
+    <parent link="right_leg_pelvis_yaw"/>
+    <child link="right_leg_pelvis_roll"/>
+    <origin xyz="0 0 0" rpy="0 0 0" />
+    <axis xyz="0 0 1" />
+    <limit effort="2.500" velocity="6.0" lower="-0.93" upper="1.0" />
+    <dynamics damping="0.3"/>
+  </joint>
+
+  <transmission name="tran_pelvis_roll_r">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j_pelvis_roll_r"/>
+    <actuator name="pelvis_roll_r_motor">
+      <hardwareInterface>EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <joint name="j_pelvis_pitch_r" type="revolute">
+    <parent link="right_leg_pelvis_roll"/>
+    <child link="right_leg_pelvis_pitch"/>
+    <origin xyz="0 0 0" rpy="0 0 0" />
+    <axis xyz="1 0 0" />
+    <limit effort="2.500" velocity="6.0" lower="-1.68" upper="0.5" />
+    <dynamics damping="0.3"/>
+  </joint>
+
+  <transmission name="tran_pelvis_pitch_r">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j_pelvis_pitch_r"/>
+    <actuator name="pelvis_pitch_r_motor">
+      <hardwareInterface>EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <joint name="j_knee_r" type="revolute">
+    <parent link="right_leg_pelvis_pitch"/>
+    <child link="right_leg_knee"/>
+    <origin xyz="0 -0.075 -0.01488" rpy="0 0 0" />
+    <axis xyz="1 0 0" />
+    <limit effort="2.500" velocity="6.0" lower="-0.03" upper="2.25" />
+    <dynamics damping="0.3"/>
+  </joint>
+
+  <transmission name="tran_knee_r">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j_knee_r"/>
+    <actuator name="knee_r_motor">
+      <hardwareInterface>EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <joint name="j_ankle_pitch_r" type="revolute">
+    <parent link="right_leg_knee"/>
+    <child link="right_leg_ankle_pitch"/>
+    <origin xyz="0 -0.075 0.01488" rpy="0 0 0" />
+    <axis xyz="1 0 0" />
+    <limit effort="2.500" velocity="6.0" lower="-1.22" upper="1.39" />
+    <dynamics damping="0.3"/>
+  </joint>
+
+  <transmission name="tran_ankle_pitch_r">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j_ankle_pitch_r"/>
+    <actuator name="ankle_pitch_r_motor">
+      <hardwareInterface>EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <joint name="j_ankle_roll_r" type="revolute">
+    <parent link="right_leg_ankle_pitch"/>
+    <child link="right_leg_ankle_roll"/>
+    <origin xyz="0 0 0" rpy="0 0 0" />
+    <axis xyz="0 0 1" />
+    <limit effort="2.500" velocity="6.0" lower="-1.02" upper="0.6" />
+    <dynamics damping="0.3"/>
+  </joint>
+
+  <transmission name="tran_ankle_roll_r">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j_ankle_roll_r"/>
+    <actuator name="ankle_roll_r_motor">
+      <hardwareInterface>EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
 </robot>
diff --git a/bioloid_gazebo/CMakeLists.txt b/bioloid_gazebo/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..92f350c26d6d08eb8d1c91dbe7798e579761bb19
--- /dev/null
+++ b/bioloid_gazebo/CMakeLists.txt
@@ -0,0 +1,156 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(bioloid_gazebo)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependencies might have been
+##     pulled in transitively but can be declared for certainty nonetheless:
+##     * add a build_depend tag for "message_generation"
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES bioloid_gazebo
+#  CATKIN_DEPENDS other_catkin_pkg
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+
+## Declare a cpp library
+# add_library(bioloid_gazebo
+#   src/${PROJECT_NAME}/bioloid_gazebo.cpp
+# )
+
+## Declare a cpp executable
+# add_executable(bioloid_gazebo_node src/bioloid_gazebo_node.cpp)
+
+## Add cmake target dependencies of the executable/library
+## as an example, message headers may need to be generated before nodes
+# add_dependencies(bioloid_gazebo_node bioloid_gazebo_generate_messages_cpp)
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(bioloid_gazebo_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS bioloid_gazebo bioloid_gazebo_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_bioloid_gazebo.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/bioloid_gazebo/launch/bioloid_gazebo.launch b/bioloid_gazebo/launch/bioloid_gazebo.launch
new file mode 100644
index 0000000000000000000000000000000000000000..6eb3777efe7adea724caca9d127da18fc133f781
--- /dev/null
+++ b/bioloid_gazebo/launch/bioloid_gazebo.launch
@@ -0,0 +1,15 @@
+<launch>
+  <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="$(find bioloid_gazebo)/worlds/bioloid.world"/>
+    <arg name="paused" value="true"/>
+    <!-- more default parameters can be changed here -->
+  </include>
+
+  <include file="$(find bioloid_description)/launch/bioloid_base.launch"/>
+
+  <!-- Spawn a robot into Gazebo -->
+  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model bioloid -x 0 -y 0 -z 0.35 -R 1.6 -P 0 -Y 0"/>
+
+</launch>
+
diff --git a/bioloid_gazebo/package.xml b/bioloid_gazebo/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..ea33ebdd3843ade07bd9c27b4bdc7321a80bd43b
--- /dev/null
+++ b/bioloid_gazebo/package.xml
@@ -0,0 +1,53 @@
+<?xml version="1.0"?>
+<package>
+  <name>bioloid_gazebo</name>
+  <version>0.0.0</version>
+  <description>The bioloid_gazebo package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="sergi@todo.todo">sergi</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/bioloid_gazebo</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- You can specify that this package is a metapackage here: -->
+    <!-- <metapackage/> -->
+
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
\ No newline at end of file
diff --git a/bioloid_gazebo/worlds/bioloid.world b/bioloid_gazebo/worlds/bioloid.world
new file mode 100644
index 0000000000000000000000000000000000000000..cb35e70bb2c4acae1e6d3d78b93479149f3af3e0
--- /dev/null
+++ b/bioloid_gazebo/worlds/bioloid.world
@@ -0,0 +1,22 @@
+<?xml version="1.0" ?>
+<sdf version="1.4">
+  <world name="default">
+    <physics type="ode">
+      <max_step_size>0.001</max_step_size>
+      <gravity>0 0 -9.8</gravity>
+      <ode>
+        <solver>
+          <type>quick</type>
+          <min_step_size>0.001</min_step_size>
+          <iters>500</iters>
+        </solver>
+      </ode>
+    </physics>
+    <include>
+      <uri>model://ground_plane</uri>
+    </include>
+    <include>
+      <uri>model://sun</uri>
+    </include>
+  </world>
+</sdf>