diff --git a/darwin_apps/launch/darwin_cpp.launch b/darwin_apps/launch/darwin_cpp.launch
index b250cc9a5d04562de6653f830e87ff7ac55825b1..0c37f2b2fda11eaa7cf276eb3a568c75fce9027a 100644
--- a/darwin_apps/launch/darwin_cpp.launch
+++ b/darwin_apps/launch/darwin_cpp.launch
@@ -7,8 +7,8 @@
     <arg name="robot" value="$(arg robot)" />
   </include>
 
-<!--  <include file="$(find bioloid_description)/launch/obstacles_env.launch">
+  <include file="$(find bioloid_description)/launch/obstacles_env.launch">
     <arg name="environment" value="$(arg environment)" />
-  </include>-->
+  </include>
 
 </launch>
diff --git a/darwin_control/config/darwin_cpp.yaml b/darwin_control/config/darwin_cpp.yaml
index b4138f2f2bbceda461da6f43b762d9b1bb4ae1a2..5fa9c84ac877f8028fcb2f91ad850d4a2b7df8b8 100644
--- a/darwin_control/config/darwin_cpp.yaml
+++ b/darwin_control/config/darwin_cpp.yaml
@@ -7,9 +7,7 @@ darwin:
   darwin_controller_cpp:
     left_sensor_foot_present: true
     right_sensor_foot_present: true
-    serial_device: '/dev/ttyUSB1'
-    baudrate: 115200
-    slave_address: 2
+    serial_device: '/dev/pts/19'
     type: effort_controllers/DarwinControllerCPP
     joints:
       - j_shoulder_pitch_r
diff --git a/darwin_description/config/darwin_rviz.rviz b/darwin_description/config/darwin_rviz.rviz
index c2de727a23005d4968bc263e22fffcab493450d8..ff2c2c5984f93e264ece48cec0a5bc7bcdab428a 100644
--- a/darwin_description/config/darwin_rviz.rviz
+++ b/darwin_description/config/darwin_rviz.rviz
@@ -9,7 +9,7 @@ Panels:
         - /TF1/Frames1
         - /TF1/Tree1
         - /Environment1
-      Splitter Ratio: 0.744966
+      Splitter Ratio: 0.744965971
     Tree Height: 655
   - Class: rviz/Selection
     Name: Selection
@@ -19,7 +19,7 @@ Panels:
       - /2D Nav Goal1
       - /Publish Point1
     Name: Tool Properties
-    Splitter Ratio: 0.588679
+    Splitter Ratio: 0.588679016
   - Class: rviz/Views
     Expanded:
       - /Current View1
@@ -39,7 +39,7 @@ Visualization Manager:
       Color: 160; 160; 164
       Enabled: true
       Line Style:
-        Line Width: 0.03
+        Line Width: 0.0299999993
         Value: Lines
       Name: Grid
       Normal Cell Count: 0
@@ -104,6 +104,46 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        left_down_left_front_ir_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_down_left_middle_ir_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_down_left_rear_ir_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_down_right_front_ir_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_down_right_middle_ir_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_down_right_rear_ir_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_front_left_ir_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_front_right_ir_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
         left_leg_ankle_pitch:
           Alpha: 1
           Show Axes: false
@@ -154,6 +194,46 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        right_down_left_front_ir_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_down_left_middle_ir_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_down_left_rear_ir_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_down_right_front_ir_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_down_right_middle_ir_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_down_right_rear_ir_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_front_left_ir_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_front_right_ir_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
         right_leg_ankle_pitch:
           Alpha: 1
           Show Axes: false
@@ -200,38 +280,6 @@ Visualization Manager:
       Frame Timeout: 15
       Frames:
         All Enabled: true
-        E1:
-          Value: true
-        E1L:
-          Value: true
-        E1R:
-          Value: true
-        N1:
-          Value: true
-        N1L:
-          Value: true
-        N1R:
-          Value: true
-        N1_cam:
-          Value: true
-        N1_det:
-          Value: true
-        S1:
-          Value: true
-        S1L:
-          Value: true
-        S1R:
-          Value: true
-        W1:
-          Value: true
-        W1L:
-          Value: true
-        W1R:
-          Value: true
-        W1_cam:
-          Value: true
-        W1_det:
-          Value: true
         darwin/base_footprint:
           Value: true
         darwin/base_link:
@@ -252,6 +300,22 @@ Visualization Manager:
           Value: true
         darwin/left_arm_low:
           Value: true
+        darwin/left_down_left_front_ir_link:
+          Value: true
+        darwin/left_down_left_middle_ir_link:
+          Value: true
+        darwin/left_down_left_rear_ir_link:
+          Value: true
+        darwin/left_down_right_front_ir_link:
+          Value: true
+        darwin/left_down_right_middle_ir_link:
+          Value: true
+        darwin/left_down_right_rear_ir_link:
+          Value: true
+        darwin/left_front_left_ir_link:
+          Value: true
+        darwin/left_front_right_ir_link:
+          Value: true
         darwin/left_leg_ankle_pitch:
           Value: true
         darwin/left_leg_ankle_roll:
@@ -274,6 +338,22 @@ Visualization Manager:
           Value: true
         darwin/right_arm_low:
           Value: true
+        darwin/right_down_left_front_ir_link:
+          Value: true
+        darwin/right_down_left_middle_ir_link:
+          Value: true
+        darwin/right_down_left_rear_ir_link:
+          Value: true
+        darwin/right_down_right_front_ir_link:
+          Value: true
+        darwin/right_down_right_middle_ir_link:
+          Value: true
+        darwin/right_down_right_rear_ir_link:
+          Value: true
+        darwin/right_front_left_ir_link:
+          Value: true
+        darwin/right_front_right_ir_link:
+          Value: true
         darwin/right_leg_ankle_pitch:
           Value: true
         darwin/right_leg_ankle_roll:
@@ -288,100 +368,78 @@ Visualization Manager:
           Value: true
         darwin/right_shoulder:
           Value: true
-        obstacle_base_fence_link:
-          Value: true
-        obstacle_base_link:
-          Value: true
-        station_connector_link:
-          Value: true
-        station_far_qr_link:
-          Value: true
-        station_near_qr_link:
-          Value: true
-        station_support_link:
-          Value: true
-      Marker Scale: 0.2
+      Marker Scale: 0.200000003
       Name: TF
       Show Arrows: true
       Show Axes: true
       Show Names: true
       Tree:
-        darwin/base_link:
-          darwin/darwin_accel:
-            {}
-          darwin/darwin_gyro:
-            {}
-          darwin/darwin_imu_link:
-            {}
-          darwin/left_leg_pelvis_yaw:
-            darwin/left_leg_pelvis_roll:
-              darwin/left_leg_pelvis_pitch:
-                darwin/left_leg_knee:
-                  darwin/left_leg_ankle_pitch:
-                    darwin/left_leg_ankle_roll:
-                      {}
-          darwin/left_shoulder:
-            darwin/left_arm_high:
-              darwin/left_arm_low:
-                {}
-          darwin/neck:
-            darwin/head:
-              darwin/darwin_cam_link:
-                darwin/darwin_cam_optical_link:
-                  N1_cam:
-                    {}
-                  N1_det:
-                    {}
-                  W1_cam:
-                    {}
-                  W1_det:
-                    {}
-          darwin/right_leg_pelvis_yaw:
-            darwin/right_leg_pelvis_roll:
-              darwin/right_leg_pelvis_pitch:
-                darwin/right_leg_knee:
-                  darwin/right_leg_ankle_pitch:
-                    darwin/right_leg_ankle_roll:
-                      {}
-          darwin/right_shoulder:
-            darwin/right_arm_high:
-              darwin/right_arm_low:
-                {}
         darwin/odom:
           darwin/base_footprint:
-            {}
-        obstacle_base_link:
-          obstacle_base_fence_link:
-            {}
-          station_support_link:
-            station_connector_link:
-              {}
-            station_far_qr_link:
-              E1:
+            darwin/base_link:
+              darwin/darwin_accel:
                 {}
-              N1:
+              darwin/darwin_gyro:
                 {}
-              S1:
-                {}
-              W1:
-                {}
-            station_near_qr_link:
-              E1L:
-                {}
-              E1R:
-                {}
-              N1L:
-                {}
-              N1R:
-                {}
-              S1L:
-                {}
-              S1R:
-                {}
-              W1L:
-                {}
-              W1R:
+              darwin/darwin_imu_link:
                 {}
+              darwin/left_leg_pelvis_yaw:
+                darwin/left_leg_pelvis_roll:
+                  darwin/left_leg_pelvis_pitch:
+                    darwin/left_leg_knee:
+                      darwin/left_leg_ankle_pitch:
+                        darwin/left_leg_ankle_roll:
+                          darwin/left_down_left_front_ir_link:
+                            {}
+                          darwin/left_down_left_middle_ir_link:
+                            {}
+                          darwin/left_down_left_rear_ir_link:
+                            {}
+                          darwin/left_down_right_front_ir_link:
+                            {}
+                          darwin/left_down_right_middle_ir_link:
+                            {}
+                          darwin/left_down_right_rear_ir_link:
+                            {}
+                          darwin/left_front_left_ir_link:
+                            {}
+                          darwin/left_front_right_ir_link:
+                            {}
+              darwin/left_shoulder:
+                darwin/left_arm_high:
+                  darwin/left_arm_low:
+                    {}
+              darwin/neck:
+                darwin/head:
+                  darwin/darwin_cam_link:
+                    darwin/darwin_cam_optical_link:
+                      {}
+              darwin/right_leg_pelvis_yaw:
+                darwin/right_leg_pelvis_roll:
+                  darwin/right_leg_pelvis_pitch:
+                    darwin/right_leg_knee:
+                      darwin/right_leg_ankle_pitch:
+                        darwin/right_leg_ankle_roll:
+                          darwin/right_down_left_front_ir_link:
+                            {}
+                          darwin/right_down_left_middle_ir_link:
+                            {}
+                          darwin/right_down_left_rear_ir_link:
+                            {}
+                          darwin/right_down_right_front_ir_link:
+                            {}
+                          darwin/right_down_right_middle_ir_link:
+                            {}
+                          darwin/right_down_right_rear_ir_link:
+                            {}
+                          darwin/right_front_left_ir_link:
+                            {}
+                          darwin/right_front_right_ir_link:
+                            {}
+              darwin/right_shoulder:
+                darwin/right_arm_high:
+                  darwin/right_arm_low:
+                    {}
       Update Interval: 0
       Value: true
     - Alpha: 1
@@ -390,100 +448,10 @@ Visualization Manager:
       Enabled: true
       Links:
         All Links Enabled: true
-        E1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        E1L:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        E1R:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
         Expand Joint Details: false
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
-        N1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        N1L:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        N1R:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        S1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        S1L:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        S1R:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        W1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        W1L:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        W1R:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        obstacle_base_fence_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        obstacle_base_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        station_connector_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        station_far_qr_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        station_near_qr_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        station_support_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
       Name: Environment
       Robot Description: /charge_env/robot_description
       TF Prefix: ""
@@ -493,7 +461,8 @@ Visualization Manager:
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
-    Fixed Frame: obstacle_base_link
+    Default Light: true
+    Fixed Frame: darwin/base_footprint
     Frame Rate: 30
   Name: root
   Tools:
@@ -514,22 +483,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 4.15359
+      Distance: 2.83055544
       Enable Stereo Rendering:
-        Stereo Eye Separation: 0.06
+        Stereo Eye Separation: 0.0599999987
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.6497
-        Y: 0.53254
-        Z: 0.117795
+        X: 0.174162328
+        Y: -0.0848779902
+        Z: -0.0313329101
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.0500000007
+      Invert Z Axis: false
       Name: Current View
-      Near Clip Distance: 0.01
-      Pitch: 1.0202
+      Near Clip Distance: 0.00999999978
+      Pitch: 0.130199939
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 2.80226
+      Yaw: 3.13725877
     Saved: ~
 Window Geometry:
   Displays:
@@ -537,7 +509,7 @@ Window Geometry:
   Height: 936
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001ce0000031efc0200000007fb0000001200530065006c0065006300740069006f006e00000000280000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000031e000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000031efc0200000004fb0000001e0054006f006f006c002000500072006f007000650072007400690065007301000000280000006e0000006400fffffffb0000000a00560069006500770073010000009c000002aa000000b000fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073010000031b000000a30000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004bf0000003efc0100000002fb0000000800540069006d00650100000000000004bf000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000001d60000031e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001ce0000031efc0200000007fb0000001200530065006c0065006300740069006f006e00000000280000009b0000006100fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000031e000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000031efc0200000004fb0000001e0054006f006f006c002000500072006f007000650072007400690065007301000000280000006e0000006100fffffffb0000000a00560069006500770073010000009c000002aa000000ad00fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073010000031b000000a30000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004bf0000003efc0100000002fb0000000800540069006d00650100000000000004bf0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000001d60000031e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -547,5 +519,5 @@ Window Geometry:
   Views:
     collapsed: false
   Width: 1215
-  X: 55
-  Y: 14
+  X: 65
+  Y: 24
diff --git a/darwin_description/urdf/charge/charge_station.xacro b/darwin_description/urdf/charge/charge_station.xacro
index 556db6a9eeb6d39dcdc0a208d3acb964e599baf8..ad70631fa19527f244102e228c68e087816beb63 100644
--- a/darwin_description/urdf/charge/charge_station.xacro
+++ b/darwin_description/urdf/charge/charge_station.xacro
@@ -172,8 +172,6 @@
       <minDepth>0.0</minDepth>
       <mu1>0.200000</mu1>
       <mu2>0.200000</mu2>
-      <kp>0.0</kp>
-      <kd>0.0</kd>
     </gazebo>
 
     <gazebo reference="${name}_far_qr_link">
@@ -185,8 +183,6 @@
       <minDepth>0.0</minDepth>
       <mu1>0.200000</mu1>
       <mu2>0.200000</mu2>
-      <kp>0.0</kp>
-      <kd>0.0</kd>
     </gazebo>
 
     <gazebo reference="${name}_connector_link">
@@ -198,8 +194,6 @@
       <minDepth>0.0</minDepth>
       <mu1>0.200000</mu1>
       <mu2>0.200000</mu2>
-      <kp>0.0</kp>
-      <kd>0.0</kd>
     </gazebo>
 
     <gazebo reference="${name}_near_qr_link">
@@ -211,8 +205,6 @@
       <minDepth>0.0</minDepth>
       <mu1>0.200000</mu1>
       <mu2>0.200000</mu2>
-      <kp>0.0</kp>
-      <kd>0.0</kd>
     </gazebo>
 
   </xacro:macro>
diff --git a/darwin_description/urdf/darwin.gazebo b/darwin_description/urdf/darwin.gazebo
index d714feeee582f2a9c9154f7e2dad1cd11af34ff8..bdcb8b7cf106bff7862a080debeed23cedf62657 100644
--- a/darwin_description/urdf/darwin.gazebo
+++ b/darwin_description/urdf/darwin.gazebo
@@ -81,8 +81,6 @@
     <minDepth>0.0</minDepth>
     <mu1>0.200000</mu1>
     <mu2>0.200000</mu2>
-    <kp>0.0</kp>
-    <kd>0.0</kd>
   </gazebo>
 
   <gazebo reference="neck">
@@ -94,8 +92,6 @@
     <minDepth>0.0</minDepth>
     <mu1>0.200000</mu1>
     <mu2>0.200000</mu2>
-    <kp>0.0</kp>
-    <kd>0.0</kd>
   </gazebo>
 
   <gazebo reference="head">
@@ -107,8 +103,6 @@
     <minDepth>0.0</minDepth>
     <mu1>0.200000</mu1>
     <mu2>0.200000</mu2>
-    <kp>0.0</kp>
-    <kd>0.0</kd>
   </gazebo>
 
   <gazebo reference="left_shoulder">
@@ -120,8 +114,6 @@
     <minDepth>0.0</minDepth>
     <mu1>0.200000</mu1>
     <mu2>0.200000</mu2>
-    <kp>0.0</kp>
-    <kd>0.0</kd>
   </gazebo>
 
   <gazebo reference="left_arm_high">
@@ -133,8 +125,6 @@
     <minDepth>0.0</minDepth>
     <mu1>0.200000</mu1>
     <mu2>0.200000</mu2>
-    <kp>0.0</kp>
-    <kd>0.0</kd>
   </gazebo>
 
   <gazebo reference="right_shoulder">
@@ -146,8 +136,6 @@
     <minDepth>0.0</minDepth>
     <mu1>0.200000</mu1>
     <mu2>0.200000</mu2>
-    <kp>0.0</kp>
-    <kd>0.0</kd>
   </gazebo>
 
   <gazebo reference="right_arm_high">
@@ -159,8 +147,6 @@
     <minDepth>0.0</minDepth>
     <mu1>0.200000</mu1>
     <mu2>0.200000</mu2>
-    <kp>0.0</kp>
-    <kd>0.0</kd>
   </gazebo>
 
   <gazebo reference="left_leg_pelvis_pitch">
@@ -172,8 +158,6 @@
     <minDepth>0.0</minDepth>
     <mu1>0.200000</mu1>
     <mu2>0.200000</mu2>
-    <kp>0.0</kp>
-    <kd>0.0</kd>
   </gazebo>
 
   <gazebo reference="left_leg_pelvis_roll">
@@ -185,8 +169,6 @@
     <minDepth>0.0</minDepth>
     <mu1>0.200000</mu1>
     <mu2>0.200000</mu2>
-    <kp>0.0</kp>
-    <kd>0.0</kd>
   </gazebo>
 
   <gazebo reference="left_leg_pelvis_pitch">
@@ -198,8 +180,6 @@
     <minDepth>0.0</minDepth>
     <mu1>0.200000</mu1>
     <mu2>0.200000</mu2>
-    <kp>0.0</kp>
-    <kd>0.0</kd>
   </gazebo>
 
   <gazebo reference="left_leg_knee">
@@ -211,8 +191,6 @@
     <minDepth>0.0</minDepth>
     <mu1>0.200000</mu1>
     <mu2>0.200000</mu2>
-    <kp>0.0</kp>
-    <kd>0.0</kd>
   </gazebo>
 
   <gazebo reference="left_leg_ankle_pitch">
@@ -224,8 +202,6 @@
     <minDepth>0.0</minDepth>
     <mu1>0.200000</mu1>
     <mu2>0.200000</mu2>
-    <kp>0.0</kp>
-    <kd>0.0</kd>
   </gazebo>
 
   <gazebo reference="right_leg_pelvis_pitch">
@@ -237,8 +213,6 @@
     <minDepth>0.0</minDepth>
     <mu1>0.200000</mu1>
     <mu2>0.200000</mu2>
-    <kp>0.0</kp>
-    <kd>0.0</kd>
   </gazebo>
 
   <gazebo reference="right_leg_pelvis_roll">
@@ -250,8 +224,6 @@
     <minDepth>0.0</minDepth>
     <mu1>0.200000</mu1>
     <mu2>0.200000</mu2>
-    <kp>0.0</kp>
-    <kd>0.0</kd>
   </gazebo>
 
   <gazebo reference="right_leg_pelvis_pitch">
@@ -263,8 +235,6 @@
     <minDepth>0.0</minDepth>
     <mu1>0.200000</mu1>
     <mu2>0.200000</mu2>
-    <kp>0.0</kp>
-    <kd>0.0</kd>
   </gazebo>
 
   <gazebo reference="right_leg_knee">
@@ -276,8 +246,6 @@
     <minDepth>0.0</minDepth>
     <mu1>0.200000</mu1>
     <mu2>0.200000</mu2>
-    <kp>0.0</kp>
-    <kd>0.0</kd>
   </gazebo>
 
   <gazebo reference="right_leg_ankle_pitch">
@@ -289,8 +257,6 @@
     <minDepth>0.0</minDepth>
     <mu1>0.200000</mu1>
     <mu2>0.200000</mu2>
-    <kp>0.0</kp>
-    <kd>0.0</kd>
   </gazebo>
 
   <gazebo reference="j_tilt">
diff --git a/darwin_description/urdf/gripper.gazebo b/darwin_description/urdf/gripper.gazebo
index 56857fcda98762e5465a76a3a65e35fc60f644eb..b935a753ab4c18e766e87c3c327df6090a142be9 100644
--- a/darwin_description/urdf/gripper.gazebo
+++ b/darwin_description/urdf/gripper.gazebo
@@ -16,8 +16,6 @@
       <minDepth>0.0</minDepth>
       <mu1>0.200000</mu1>
       <mu2>0.200000</mu2>
-      <kp>0.0</kp>
-      <kd>0.0</kd>
     </gazebo>
 
     <gazebo reference="j_elbow_${name}">
@@ -39,8 +37,6 @@
       <minDepth>0.0</minDepth>
       <mu1>0.200000</mu1>
       <mu2>0.200000</mu2>
-      <kp>0.0</kp>
-      <kd>0.0</kd>
     </gazebo>
 
     <gazebo reference="j_wrist_${name}">
@@ -62,8 +58,6 @@
       <minDepth>0.0</minDepth>
       <mu1>0.200000</mu1>
       <mu2>0.200000</mu2>
-      <kp>0.0</kp>
-      <kd>0.0</kd>
     </gazebo>
 
     <gazebo reference="j_finger1_seg1_${name}">
@@ -85,8 +79,6 @@
       <minDepth>0.0</minDepth>
       <mu1>0.200000</mu1>
       <mu2>0.200000</mu2>
-      <kp>0.0</kp>
-      <kd>0.0</kd>
     </gazebo>
 
     <gazebo reference="j_finger1_seg2_${name}">
@@ -108,8 +100,6 @@
       <minDepth>0.0</minDepth>
       <mu1>0.200000</mu1>
       <mu2>0.200000</mu2>
-      <kp>0.0</kp>
-      <kd>0.0</kd>
     </gazebo>
 
     <gazebo reference="j_finger1_seg3_${name}">
@@ -131,8 +121,6 @@
       <minDepth>0.0</minDepth>
       <mu1>0.200000</mu1>
       <mu2>0.200000</mu2>
-      <kp>0.0</kp>
-      <kd>0.0</kd>
     </gazebo>
 
     <gazebo reference="j_finger2_seg1_${name}">
@@ -154,8 +142,6 @@
       <minDepth>0.0</minDepth>
       <mu1>0.200000</mu1>
       <mu2>0.200000</mu2>
-      <kp>0.0</kp>
-      <kd>0.0</kd>
     </gazebo>
 
     <gazebo reference="j_finger2_seg2_${name}">
@@ -177,8 +163,6 @@
       <minDepth>0.0</minDepth>
       <mu1>0.200000</mu1>
       <mu2>0.200000</mu2>
-      <kp>0.0</kp>
-      <kd>0.0</kd>
     </gazebo>
 
     <gazebo reference="${name}_finger3_seg1">
@@ -194,8 +178,6 @@
       <minDepth>0.0</minDepth>
       <mu1>0.200000</mu1>
       <mu2>0.200000</mu2>
-      <kp>0.0</kp>
-      <kd>0.0</kd>
     </gazebo>
 
     <gazebo reference="j_finger3_seg1_${name}">
@@ -217,8 +199,6 @@
       <minDepth>0.0</minDepth>
       <mu1>0.200000</mu1>
       <mu2>0.200000</mu2>
-      <kp>0.0</kp>
-      <kd>0.0</kd>
     </gazebo>
 
     <gazebo reference="j_finger3_seg2_${name}">
@@ -240,8 +220,6 @@
       <minDepth>0.0</minDepth>
       <mu1>0.200000</mu1>
       <mu2>0.200000</mu2>
-      <kp>0.0</kp>
-      <kd>0.0</kd>
     </gazebo>
 
   </xacro:macro>
diff --git a/darwin_description/urdf/gripper_sc.gazebo b/darwin_description/urdf/gripper_sc.gazebo
index d188a0cf1489dbbd8b0ab62bdbe6c0ed5c74c1fd..c6b54d23aaf68936b070a2628585ceefe938fa37 100644
--- a/darwin_description/urdf/gripper_sc.gazebo
+++ b/darwin_description/urdf/gripper_sc.gazebo
@@ -16,8 +16,6 @@
       <minDepth>0.0</minDepth>
       <mu1>0.200000</mu1>
       <mu2>0.200000</mu2>
-      <kp>0.0</kp>
-      <kd>0.0</kd>
     </gazebo>
 
     <gazebo reference="j_elbow_${name}">
@@ -39,8 +37,6 @@
       <minDepth>0.0</minDepth>
       <mu1>0.200000</mu1>
       <mu2>0.200000</mu2>
-      <kp>0.0</kp>
-      <kd>0.0</kd>
     </gazebo>
 
     <gazebo reference="j_gripper_bot_${name}">
@@ -62,8 +58,6 @@
       <minDepth>0.0</minDepth>
       <mu1>0.200000</mu1>
       <mu2>0.200000</mu2>
-      <kp>0.0</kp>
-      <kd>0.0</kd>
     </gazebo>
 
     <gazebo reference="j_gripper_top_${name}">
diff --git a/darwin_description/urdf/left_foot.gazebo b/darwin_description/urdf/left_foot.gazebo
index dc76eb4086c32748612f736571bb2f7ceabfe212..f0be2f7e447f76422e28e11ce2702cd0b680cd4f 100644
--- a/darwin_description/urdf/left_foot.gazebo
+++ b/darwin_description/urdf/left_foot.gazebo
@@ -13,8 +13,6 @@
     <minDepth>0.0</minDepth>
     <mu1>1.000000</mu1>
     <mu2>1.000000</mu2>
-    <kp>0.0</kp>
-    <kd>0.0</kd>
   </gazebo>
 
   <gazebo reference="j_ankle_roll_l">
diff --git a/darwin_description/urdf/left_hand.gazebo b/darwin_description/urdf/left_hand.gazebo
index 60cbc0171ade682806ab968cc7b9fd799f7478b3..bd86a71218430dcd94b7a79338f8e1e26a571d58 100644
--- a/darwin_description/urdf/left_hand.gazebo
+++ b/darwin_description/urdf/left_hand.gazebo
@@ -15,8 +15,6 @@
     <minDepth>0.0</minDepth>
     <mu1>0.200000</mu1>
     <mu2>0.200000</mu2>
-    <kp>0.0</kp>
-    <kd>0.0</kd>
   </gazebo>
 
   <gazebo reference="j_elbow_l">
diff --git a/darwin_description/urdf/right_foot.gazebo b/darwin_description/urdf/right_foot.gazebo
index 8b477296013d3ed2ac4a9ee94f73f14e8c0fc5e3..3682735ec532dbe0c10f51a28c1902a2356d0a6e 100644
--- a/darwin_description/urdf/right_foot.gazebo
+++ b/darwin_description/urdf/right_foot.gazebo
@@ -13,8 +13,6 @@
     <minDepth>0.0</minDepth>
     <mu1>1.00000</mu1>
     <mu2>1.00000</mu2>
-    <kp>0.0</kp>
-    <kd>0.0</kd>
   </gazebo>
 
   <gazebo reference="j_ankle_roll_r">
diff --git a/darwin_description/urdf/right_hand.gazebo b/darwin_description/urdf/right_hand.gazebo
index fc48d17c96643cb89295a06b967627e07fc36655..55df05a9462bca7bc3ffbd1f99f9431e55b707f2 100644
--- a/darwin_description/urdf/right_hand.gazebo
+++ b/darwin_description/urdf/right_hand.gazebo
@@ -15,8 +15,6 @@
     <minDepth>0.0</minDepth>
     <mu1>0.200000</mu1>
     <mu2>0.200000</mu2>
-    <kp>0.0</kp>
-    <kd>0.0</kd>
   </gazebo>
 
   <gazebo reference="j_elbow_r">