diff --git a/gazebo/omniRobot_plugin/src/gazebo_omni_drive.cpp b/gazebo/omniRobot_plugin/src/gazebo_omni_drive.cpp
index 8e96de4..dffe973 100644
--- a/gazebo/omniRobot_plugin/src/gazebo_omni_drive.cpp
+++ b/gazebo/omniRobot_plugin/src/gazebo_omni_drive.cpp
@@ -393,11 +393,19 @@ void GazeboOmniRosDrivePrivate::UpdateWheelVelocities()
   double va = target_rot_;
 
   for (unsigned int i = 0; i < num_wheel_pairs_; ++i)
-  {
+  { 
+    /*
     desired_wheel_speed_[2 * i + NORTH] = (vx + vy - va * (wheel_w_separation_[i] + wheel_l_separation_[i]) / 2.0) / wheel_diameter_[i] * 2;
     desired_wheel_speed_[2 * i + WEST] = (vx - vy - va * (wheel_w_separation_[i] + wheel_l_separation_[i]) / 2.0) / wheel_diameter_[i] * 2;
     desired_wheel_speed_[2 * i + SOUTH] = (vx + vy + va * (wheel_w_separation_[i] + wheel_l_separation_[i]) / 2.0) / wheel_diameter_[i] * 2;
     desired_wheel_speed_[2 * i + EAST] = (vx - vy + va * (wheel_w_separation_[i] + wheel_l_separation_[i]) / 2.0) / wheel_diameter_[i] * 2;
+    */
+   double wl = va * (wheel_w_separation_[i] + wheel_l_separation_[i])  ;
+    desired_wheel_speed_[2 * i + NORTH] = (-wl - vy) / wheel_diameter_[i] * 2;
+    desired_wheel_speed_[2 * i + WEST] = (-wl + vx) / wheel_diameter_[i] * 2;
+    desired_wheel_speed_[2 * i + SOUTH] = (wl - vy) / wheel_diameter_[i] * 2;
+    desired_wheel_speed_[2 * i + EAST] = (wl + vx) / wheel_diameter_[i] * 2;
+
   }
 }
 
diff --git a/src/ezbot_descr_simul/description/robot_core.xacro b/src/ezbot_descr_simul/description/robot_core.xacro
index 3c93e84..d4003b2 100644
--- a/src/ezbot_descr_simul/description/robot_core.xacro
+++ b/src/ezbot_descr_simul/description/robot_core.xacro
@@ -257,7 +257,8 @@
             <surface>
                 <contact>
                 <ode>
-                    <min_depth>0.01</min_depth>
+                    <max_vel>0</max_vel>
+                    <min_depth>0.02</min_depth>
                 </ode>
                 </contact>
             </surface>
@@ -300,7 +301,9 @@
             <surface>
                 <contact>
                 <ode>
-                    <min_depth>0.01</min_depth>
+                    <max_vel>0</max_vel>
+
+                    <min_depth>0.02</min_depth>
                 </ode>
                 </contact>
             </surface>
@@ -343,7 +346,8 @@
             <surface>
                 <contact>
                 <ode>
-                    <min_depth>0.01</min_depth>
+                    <max_vel>0</max_vel>
+                    <min_depth>0.02</min_depth>
                 </ode>
                 </contact>
             </surface>
@@ -386,7 +390,8 @@
             <surface>
                 <contact>
                 <ode>
-                    <min_depth>0.01</min_depth>
+                    <max_vel>0</max_vel>
+                    <min_depth>0.02</min_depth>
                 </ode>
                 </contact>
             </surface>