diff --git a/include/teb_local_planner/omni_helper.hpp b/include/teb_local_planner/omni_helper.hpp
new file mode 100644
index 00000000..6e240669
--- /dev/null
+++ b/include/teb_local_planner/omni_helper.hpp
@@ -0,0 +1,121 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the institute nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef OMNI_HELPER_H_
+#define OMNI_HELPER_H_
+
+#include <cmath>
+#include <algorithm>
+
+namespace teb_local_planner
+{
+
+// error
+// a1: vel_x, a2: vel_y, a3: omega_z
+inline double error_0_1(const double& a1, const double& a2, const double a3) { return std::hypot(std::hypot(a1, a2), a3); }
+
+inline double error_1_1(const double& a1, const double& a2, const double a3) { return a3 - a1 + a2; }
+inline double error_1_2(const double& a1, const double& a2, const double a3) { return a3 - a1 - a2; }
+inline double error_1_3(const double& a1, const double& a2, const double a3) { return a3 + a1 - a2; }
+inline double error_1_4(const double& a1, const double& a2, const double a3) { return a3 + a1 + a2; }
+
+inline double error_2_1(const double& a1, const double& a2, const double a3) { return a3 + a2; }
+inline double error_2_2(const double& a1, const double& a2, const double a3) { return a3 - a1; }
+inline double error_2_3(const double& a1, const double& a2, const double a3) { return a3 - a2; }
+inline double error_2_4(const double& a1, const double& a2, const double a3) { return a3 + a1; }
+
+inline double error_3_1(const double& a1, const double& a2, const double a3) { return a3 + a2; }
+inline double error_3_2(const double& a1, const double& a2, const double a3) { return a3 - a1 - a2 * 0.5; }
+inline double error_3_3(const double& a1, const double& a2, const double a3) { return a3 + a1 - a2 * 0.5; }
+
+inline double error_4_1(const double& a1, const double& a2, const double a3) { return a3 + a1; }
+inline double error_4_2(const double& a1, const double& a2, const double a3) { return a3 - a1 * 0.5 + a2; }
+inline double error_4_3(const double& a1, const double& a2, const double a3) { return a3 - a1 * 0.5 - a2; }
+
+inline double error_5_1(const double& a1, const double& a2, const double a3) { return a3 - a2; }
+inline double error_5_2(const double& a1, const double& a2, const double a3) { return a3 + a1 + a2 * 0.5; }
+inline double error_5_3(const double& a1, const double& a2, const double a3) { return a3 - a1 + a2 * 0.5; }
+
+inline double error_6_1(const double& a1, const double& a2, const double a3) { return a3 - a1; }
+inline double error_6_2(const double& a1, const double& a2, const double a3) { return a3 + a1 * 0.5 - a2; }
+inline double error_6_3(const double& a1, const double& a2, const double a3) { return a3 + a1 * 0.5 + a2; }
+
+
+// weight
+// a1: vel_x, a2: vel_y, a3: omega_z
+inline double weight_0_1(const double& a1, const double& a2, const double a3) { return std::hypot(std::hypot(a1, a2), a3); }
+
+inline double weight_1_1(const double& a1, const double& a2, const double a3) { return a3 + a1 + a2; }
+inline double weight_1_2(const double& a1, const double& a2, const double a3) { return a3 + a1 + a2; }
+inline double weight_1_3(const double& a1, const double& a2, const double a3) { return a3 + a1 + a2; }
+inline double weight_1_4(const double& a1, const double& a2, const double a3) { return a3 + a1 + a2; }
+
+inline double weight_2_1(const double& a1, const double& a2, const double a3) { return a3 + a2; }
+inline double weight_2_2(const double& a1, const double& a2, const double a3) { return a3 + a1; }
+inline double weight_2_3(const double& a1, const double& a2, const double a3) { return a3 + a2; }
+inline double weight_2_4(const double& a1, const double& a2, const double a3) { return a3 + a1; }
+
+inline double weight_3_1(const double& a1, const double& a2, const double a3) { return a3 + a2; }
+inline double weight_3_2(const double& a1, const double& a2, const double a3) { return a3 + a1 + a2 * 0.5; }
+inline double weight_3_3(const double& a1, const double& a2, const double a3) { return a3 + a1 + a2 * 0.5; }
+
+inline double weight_4_1(const double& a1, const double& a2, const double a3) { return a3 + a1; }
+inline double weight_4_2(const double& a1, const double& a2, const double a3) { return a3 + a1 * 0.5 + a2; }
+inline double weight_4_3(const double& a1, const double& a2, const double a3) { return a3 + a1 * 0.5 + a2; }
+
+inline double weight_5_1(const double& a1, const double& a2, const double a3) { return a3 + a2; }
+inline double weight_5_2(const double& a1, const double& a2, const double a3) { return a3 + a1 + a2 * 0.5; }
+inline double weight_5_3(const double& a1, const double& a2, const double a3) { return a3 + a1 + a2 * 0.5; }
+
+inline double weight_6_1(const double& a1, const double& a2, const double a3) { return a3 + a1; }
+inline double weight_6_2(const double& a1, const double& a2, const double a3) { return a3 + a1 * 0.5 + a2; }
+inline double weight_6_3(const double& a1, const double& a2, const double a3) { return a3 + a1 * 0.5 + a2; }
+
+// limit
+// a1: vel_x, a2: vel_y, a3: omega_z
+inline double limit_0(const double& a1, const double& a2, const double a3) { return std::hypot(std::hypot(a1, a2), a3); }
+inline double limit_1(const double& a1, const double& a2, const double a3) { return std::fabs(a1) + std::fabs(a2) + std::fabs(a3); }
+inline double limit_2(const double& a1, const double& a2, const double a3) { return std::max(std::fabs(a1), std::fabs(a2)) + std::fabs(a3); }
+inline double limit_3(const double& a1, const double& a2, const double a3) { return std::max(std::fabs(a3 + a2), std::max(std::fabs(a3 - a1 - a2 * 0.5), std::fabs(a3 + a1 - a2 * 0.5))); }
+inline double limit_4(const double& a1, const double& a2, const double a3) { return std::max(std::fabs(a3 + a1), std::max(std::fabs(a3 - a1 * 0.5 + a2), std::fabs(a3 - a1 * 0.5 - a2))); }
+inline double limit_5(const double& a1, const double& a2, const double a3) { return std::max(std::fabs(a3 - a2), std::max(std::fabs(a3 + a1 + a2 * 0.5), std::fabs(a3 - a1 + a2 * 0.5))); }
+inline double limit_6(const double& a1, const double& a2, const double a3) { return std::max(std::fabs(a3 - a1), std::max(std::fabs(a3 + a1 * 0.5 - a2), std::fabs(a3 + a1 * 0.5 + a2))); }
+
+} // namespace teb_local_planner
+
+#endif
diff --git a/src/g2o_types/edge_acceleration.cpp b/src/g2o_types/edge_acceleration.cpp
index a035bf3e..be0daa2b 100644
--- a/src/g2o_types/edge_acceleration.cpp
+++ b/src/g2o_types/edge_acceleration.cpp
@@ -42,6 +42,7 @@
  *********************************************************************/
 
 #include <teb_local_planner/g2o_types/edge_acceleration.h>
+#include <teb_local_planner/omni_helper.hpp>
 #include <teb_local_planner/teb_config.h>
 
 namespace teb_local_planner
@@ -398,7 +399,7 @@ void EdgeAccelerationHolonomic0::computeError()
   getAccelerationHolonomicNormalized(cfg_, _vertices, a1, a2, a3);
 
   // error
-  _error[0] = penaltyBoundToInterval(std::hypot(std::hypot(a1,a2),a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[0] = penaltyBoundToInterval(error_0_1(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
 }
 
 inline void getAccelerationHolonomicNormalizedStart(const TebConfig* cfg_, const geometry_msgs::Twist* _measurement, const g2o::HyperGraph::VertexContainer _vertices, double& a1, double& a2, double& a3)
@@ -447,7 +448,7 @@ void EdgeAccelerationHolonomic0Start::computeError()
   getAccelerationHolonomicNormalizedStart(cfg_, _measurement, _vertices, a1, a2, a3);
 
   // error
-  _error[0] = penaltyBoundToInterval(std::hypot(std::hypot(a1,a2),a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[0] = penaltyBoundToInterval(error_0_1(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
 }
 
 inline void getAccelerationHolonomicNormalizedGoal(const TebConfig* cfg_, const geometry_msgs::Twist* _measurement, const g2o::HyperGraph::VertexContainer _vertices, double& a1, double& a2, double& a3)
@@ -497,7 +498,7 @@ void EdgeAccelerationHolonomic0Goal::computeError()
   getAccelerationHolonomicNormalizedGoal(cfg_, _measurement, _vertices, a1, a2, a3);
 
   // error
-  _error[0] = penaltyBoundToInterval(std::hypot(std::hypot(a1,a2),a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[0] = penaltyBoundToInterval(error_0_1(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
 }
 
 void EdgeAccelerationHolonomic1::computeError()
@@ -507,10 +508,10 @@ void EdgeAccelerationHolonomic1::computeError()
   getAccelerationHolonomicNormalized(cfg_, _vertices, a1, a2, a3);
 
   // error
-  _error[0] = penaltyBoundToInterval(a1+a2+a3, 1.0, cfg_->optim.penalty_epsilon);
-  _error[1] = penaltyBoundToInterval(a1+a2-a3, 1.0, cfg_->optim.penalty_epsilon);
-  _error[2] = penaltyBoundToInterval(a1-a2+a3, 1.0, cfg_->optim.penalty_epsilon);
-  _error[3] = penaltyBoundToInterval(a1-a2-a3, 1.0, cfg_->optim.penalty_epsilon);
+  _error[0] = penaltyBoundToInterval(error_1_1(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[1] = penaltyBoundToInterval(error_1_2(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[2] = penaltyBoundToInterval(error_1_3(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[3] = penaltyBoundToInterval(error_1_4(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
 }
 
 void EdgeAccelerationHolonomic1Start::computeError()
@@ -520,10 +521,10 @@ void EdgeAccelerationHolonomic1Start::computeError()
   getAccelerationHolonomicNormalizedStart(cfg_, _measurement, _vertices, a1, a2, a3);
 
   // error
-  _error[0] = penaltyBoundToInterval(a1+a2+a3, 1.0, cfg_->optim.penalty_epsilon);
-  _error[1] = penaltyBoundToInterval(a1+a2-a3, 1.0, cfg_->optim.penalty_epsilon);
-  _error[2] = penaltyBoundToInterval(a1-a2+a3, 1.0, cfg_->optim.penalty_epsilon);
-  _error[3] = penaltyBoundToInterval(a1-a2-a3, 1.0, cfg_->optim.penalty_epsilon);
+  _error[0] = penaltyBoundToInterval(error_1_1(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[1] = penaltyBoundToInterval(error_1_2(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[2] = penaltyBoundToInterval(error_1_3(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[3] = penaltyBoundToInterval(error_1_4(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
 }
 
 void EdgeAccelerationHolonomic1Goal::computeError()
@@ -533,10 +534,10 @@ void EdgeAccelerationHolonomic1Goal::computeError()
   getAccelerationHolonomicNormalizedGoal(cfg_, _measurement, _vertices, a1, a2, a3);
 
   // error
-  _error[0] = penaltyBoundToInterval(a1+a2+a3, 1.0, cfg_->optim.penalty_epsilon);
-  _error[1] = penaltyBoundToInterval(a1+a2-a3, 1.0, cfg_->optim.penalty_epsilon);
-  _error[2] = penaltyBoundToInterval(a1-a2+a3, 1.0, cfg_->optim.penalty_epsilon);
-  _error[3] = penaltyBoundToInterval(a1-a2-a3, 1.0, cfg_->optim.penalty_epsilon);
+  _error[0] = penaltyBoundToInterval(error_1_1(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[1] = penaltyBoundToInterval(error_1_2(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[2] = penaltyBoundToInterval(error_1_3(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[3] = penaltyBoundToInterval(error_1_4(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
 }
 
 void EdgeAccelerationHolonomic2::computeError()
@@ -546,10 +547,10 @@ void EdgeAccelerationHolonomic2::computeError()
   getAccelerationHolonomicNormalized(cfg_, _vertices, a1, a2, a3);
 
   // error
-  _error[0] = penaltyBoundToInterval(a1+a3, 1.0, cfg_->optim.penalty_epsilon);
-  _error[1] = penaltyBoundToInterval(a1-a3, 1.0, cfg_->optim.penalty_epsilon);
-  _error[2] = penaltyBoundToInterval(a2+a3, 1.0, cfg_->optim.penalty_epsilon);
-  _error[3] = penaltyBoundToInterval(a2-a3, 1.0, cfg_->optim.penalty_epsilon);
+  _error[0] = penaltyBoundToInterval(error_2_1(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[1] = penaltyBoundToInterval(error_2_2(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[2] = penaltyBoundToInterval(error_2_3(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[3] = penaltyBoundToInterval(error_2_4(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
 }
 
 void EdgeAccelerationHolonomic2Start::computeError()
@@ -559,10 +560,10 @@ void EdgeAccelerationHolonomic2Start::computeError()
   getAccelerationHolonomicNormalizedStart(cfg_, _measurement, _vertices, a1, a2, a3);
 
   // error
-  _error[0] = penaltyBoundToInterval(a1+a3, 1.0, cfg_->optim.penalty_epsilon);
-  _error[1] = penaltyBoundToInterval(a1-a3, 1.0, cfg_->optim.penalty_epsilon);
-  _error[2] = penaltyBoundToInterval(a2+a3, 1.0, cfg_->optim.penalty_epsilon);
-  _error[3] = penaltyBoundToInterval(a2-a3, 1.0, cfg_->optim.penalty_epsilon);
+  _error[0] = penaltyBoundToInterval(error_2_1(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[1] = penaltyBoundToInterval(error_2_2(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[2] = penaltyBoundToInterval(error_2_3(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[3] = penaltyBoundToInterval(error_2_4(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
 }
 
 void EdgeAccelerationHolonomic2Goal::computeError()
@@ -572,10 +573,10 @@ void EdgeAccelerationHolonomic2Goal::computeError()
   getAccelerationHolonomicNormalizedGoal(cfg_, _measurement, _vertices, a1, a2, a3);
 
   // error
-  _error[0] = penaltyBoundToInterval(a1+a3, 1.0, cfg_->optim.penalty_epsilon);
-  _error[1] = penaltyBoundToInterval(a1-a3, 1.0, cfg_->optim.penalty_epsilon);
-  _error[2] = penaltyBoundToInterval(a2+a3, 1.0, cfg_->optim.penalty_epsilon);
-  _error[3] = penaltyBoundToInterval(a2-a3, 1.0, cfg_->optim.penalty_epsilon);
+  _error[0] = penaltyBoundToInterval(error_2_1(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[1] = penaltyBoundToInterval(error_2_2(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[2] = penaltyBoundToInterval(error_2_3(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[3] = penaltyBoundToInterval(error_2_4(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
 }
 
 void EdgeAccelerationHolonomic3::computeError()
@@ -585,9 +586,9 @@ void EdgeAccelerationHolonomic3::computeError()
   getAccelerationHolonomicNormalized(cfg_, _vertices, a1, a2, a3);
 
   // error
-  _error[0] = penaltyBoundToInterval(a3+a2, 1.0, cfg_->optim.penalty_epsilon);
-  _error[1] = penaltyBoundToInterval(a3-a1-a2*0.5, 1.0, cfg_->optim.penalty_epsilon);
-  _error[2] = penaltyBoundToInterval(a3+a1-a2*0.5, 1.0, cfg_->optim.penalty_epsilon);
+  _error[0] = penaltyBoundToInterval(error_3_1(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[1] = penaltyBoundToInterval(error_3_2(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[2] = penaltyBoundToInterval(error_3_3(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
 }
 
 void EdgeAccelerationHolonomic3Start::computeError()
@@ -597,9 +598,9 @@ void EdgeAccelerationHolonomic3Start::computeError()
   getAccelerationHolonomicNormalizedStart(cfg_, _measurement, _vertices, a1, a2, a3);
 
   // error
-  _error[0] = penaltyBoundToInterval(a3+a2, 1.0, cfg_->optim.penalty_epsilon);
-  _error[1] = penaltyBoundToInterval(a3-a1-a2*0.5, 1.0, cfg_->optim.penalty_epsilon);
-  _error[2] = penaltyBoundToInterval(a3+a1-a2*0.5, 1.0, cfg_->optim.penalty_epsilon);
+  _error[0] = penaltyBoundToInterval(error_3_1(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[1] = penaltyBoundToInterval(error_3_2(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[2] = penaltyBoundToInterval(error_3_3(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
 }
 
 void EdgeAccelerationHolonomic3Goal::computeError()
@@ -609,9 +610,9 @@ void EdgeAccelerationHolonomic3Goal::computeError()
   getAccelerationHolonomicNormalizedGoal(cfg_, _measurement, _vertices, a1, a2, a3);
 
   // error
-  _error[0] = penaltyBoundToInterval(a3+a2, 1.0, cfg_->optim.penalty_epsilon);
-  _error[1] = penaltyBoundToInterval(a3-a1-a2*0.5, 1.0, cfg_->optim.penalty_epsilon);
-  _error[2] = penaltyBoundToInterval(a3+a1-a2*0.5, 1.0, cfg_->optim.penalty_epsilon);
+  _error[0] = penaltyBoundToInterval(error_3_1(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[1] = penaltyBoundToInterval(error_3_2(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[2] = penaltyBoundToInterval(error_3_3(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
 }
 
 void EdgeAccelerationHolonomic4::computeError()
@@ -621,9 +622,9 @@ void EdgeAccelerationHolonomic4::computeError()
   getAccelerationHolonomicNormalized(cfg_, _vertices, a1, a2, a3);
 
   // error
-  _error[0] = penaltyBoundToInterval(a3+a1, 1.0, cfg_->optim.penalty_epsilon);
-  _error[1] = penaltyBoundToInterval(a3-a1*0.5+a2, 1.0, cfg_->optim.penalty_epsilon);
-  _error[2] = penaltyBoundToInterval(a3-a1*0.5-a2, 1.0, cfg_->optim.penalty_epsilon);
+  _error[0] = penaltyBoundToInterval(error_4_1(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[1] = penaltyBoundToInterval(error_4_2(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[2] = penaltyBoundToInterval(error_4_3(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
 }
 
 void EdgeAccelerationHolonomic4Start::computeError()
@@ -633,9 +634,9 @@ void EdgeAccelerationHolonomic4Start::computeError()
   getAccelerationHolonomicNormalizedStart(cfg_, _measurement, _vertices, a1, a2, a3);
 
   // error
-  _error[0] = penaltyBoundToInterval(a3+a1, 1.0, cfg_->optim.penalty_epsilon);
-  _error[1] = penaltyBoundToInterval(a3-a1*0.5+a2, 1.0, cfg_->optim.penalty_epsilon);
-  _error[2] = penaltyBoundToInterval(a3-a1*0.5-a2, 1.0, cfg_->optim.penalty_epsilon);
+  _error[0] = penaltyBoundToInterval(error_4_1(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[1] = penaltyBoundToInterval(error_4_2(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[2] = penaltyBoundToInterval(error_4_3(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
 }
 
 void EdgeAccelerationHolonomic4Goal::computeError()
@@ -645,9 +646,9 @@ void EdgeAccelerationHolonomic4Goal::computeError()
   getAccelerationHolonomicNormalizedGoal(cfg_, _measurement, _vertices, a1, a2, a3);
 
   // error
-  _error[0] = penaltyBoundToInterval(a3+a1, 1.0, cfg_->optim.penalty_epsilon);
-  _error[1] = penaltyBoundToInterval(a3-a1*0.5+a2, 1.0, cfg_->optim.penalty_epsilon);
-  _error[2] = penaltyBoundToInterval(a3-a1*0.5-a2, 1.0, cfg_->optim.penalty_epsilon);
+  _error[0] = penaltyBoundToInterval(error_4_1(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[1] = penaltyBoundToInterval(error_4_2(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[2] = penaltyBoundToInterval(error_4_3(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
 }
 
 void EdgeAccelerationHolonomic5::computeError()
@@ -657,9 +658,9 @@ void EdgeAccelerationHolonomic5::computeError()
   getAccelerationHolonomicNormalized(cfg_, _vertices, a1, a2, a3);
 
   // error
-  _error[0] = penaltyBoundToInterval(a3-a2, 1.0, cfg_->optim.penalty_epsilon);
-  _error[1] = penaltyBoundToInterval(a3+a1+a2*0.5, 1.0, cfg_->optim.penalty_epsilon);
-  _error[2] = penaltyBoundToInterval(a3-a1+a2*0.5, 1.0, cfg_->optim.penalty_epsilon);
+  _error[0] = penaltyBoundToInterval(error_5_1(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[1] = penaltyBoundToInterval(error_5_2(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[2] = penaltyBoundToInterval(error_5_3(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
 }
 
 void EdgeAccelerationHolonomic5Start::computeError()
@@ -669,9 +670,9 @@ void EdgeAccelerationHolonomic5Start::computeError()
   getAccelerationHolonomicNormalizedStart(cfg_, _measurement, _vertices, a1, a2, a3);
 
   // error
-  _error[0] = penaltyBoundToInterval(a3-a2, 1.0, cfg_->optim.penalty_epsilon);
-  _error[1] = penaltyBoundToInterval(a3+a1+a2*0.5, 1.0, cfg_->optim.penalty_epsilon);
-  _error[2] = penaltyBoundToInterval(a3-a1+a2*0.5, 1.0, cfg_->optim.penalty_epsilon);
+  _error[0] = penaltyBoundToInterval(error_5_1(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[1] = penaltyBoundToInterval(error_5_2(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[2] = penaltyBoundToInterval(error_5_3(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
 }
 
 void EdgeAccelerationHolonomic5Goal::computeError()
@@ -681,9 +682,9 @@ void EdgeAccelerationHolonomic5Goal::computeError()
   getAccelerationHolonomicNormalizedGoal(cfg_, _measurement, _vertices, a1, a2, a3);
 
   // error
-  _error[0] = penaltyBoundToInterval(a3-a2, 1.0, cfg_->optim.penalty_epsilon);
-  _error[1] = penaltyBoundToInterval(a3+a1+a2*0.5, 1.0, cfg_->optim.penalty_epsilon);
-  _error[2] = penaltyBoundToInterval(a3-a1+a2*0.5, 1.0, cfg_->optim.penalty_epsilon);
+  _error[0] = penaltyBoundToInterval(error_5_1(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[1] = penaltyBoundToInterval(error_5_2(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[2] = penaltyBoundToInterval(error_5_3(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
 }
 
 void EdgeAccelerationHolonomic6::computeError()
@@ -693,9 +694,9 @@ void EdgeAccelerationHolonomic6::computeError()
   getAccelerationHolonomicNormalized(cfg_, _vertices, a1, a2, a3);
 
   // error
-  _error[0] = penaltyBoundToInterval(a3-a1, 1.0, cfg_->optim.penalty_epsilon);
-  _error[1] = penaltyBoundToInterval(a3+a1*0.5-a2, 1.0, cfg_->optim.penalty_epsilon);
-  _error[2] = penaltyBoundToInterval(a3+a1*0.5+a2, 1.0, cfg_->optim.penalty_epsilon);
+  _error[0] = penaltyBoundToInterval(error_6_1(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[1] = penaltyBoundToInterval(error_6_2(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[2] = penaltyBoundToInterval(error_6_3(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
 }
 
 void EdgeAccelerationHolonomic6Start::computeError()
@@ -705,9 +706,9 @@ void EdgeAccelerationHolonomic6Start::computeError()
   getAccelerationHolonomicNormalizedStart(cfg_, _measurement, _vertices, a1, a2, a3);
 
   // error
-  _error[0] = penaltyBoundToInterval(a3-a1, 1.0, cfg_->optim.penalty_epsilon);
-  _error[1] = penaltyBoundToInterval(a3+a1*0.5-a2, 1.0, cfg_->optim.penalty_epsilon);
-  _error[2] = penaltyBoundToInterval(a3+a1*0.5+a2, 1.0, cfg_->optim.penalty_epsilon);
+  _error[0] = penaltyBoundToInterval(error_6_1(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[1] = penaltyBoundToInterval(error_6_2(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[2] = penaltyBoundToInterval(error_6_3(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
 }
 
 void EdgeAccelerationHolonomic6Goal::computeError()
@@ -717,9 +718,9 @@ void EdgeAccelerationHolonomic6Goal::computeError()
   getAccelerationHolonomicNormalizedGoal(cfg_, _measurement, _vertices, a1, a2, a3);
 
   // error
-  _error[0] = penaltyBoundToInterval(a3-a1, 1.0, cfg_->optim.penalty_epsilon);
-  _error[1] = penaltyBoundToInterval(a3+a1*0.5-a2, 1.0, cfg_->optim.penalty_epsilon);
-  _error[2] = penaltyBoundToInterval(a3+a1*0.5+a2, 1.0, cfg_->optim.penalty_epsilon);
+  _error[0] = penaltyBoundToInterval(error_6_1(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[1] = penaltyBoundToInterval(error_6_2(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[2] = penaltyBoundToInterval(error_6_3(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
 }
 
 }; // end namespace
diff --git a/src/g2o_types/edge_velocity.cpp b/src/g2o_types/edge_velocity.cpp
index 05c00d29..08d715b1 100755
--- a/src/g2o_types/edge_velocity.cpp
+++ b/src/g2o_types/edge_velocity.cpp
@@ -42,6 +42,7 @@
  *********************************************************************/
 
 #include <teb_local_planner/g2o_types/edge_velocity.h>
+#include <teb_local_planner/omni_helper.hpp>
 #include <teb_local_planner/teb_config.h>
 #include <iostream>
 
@@ -150,7 +151,7 @@ void EdgeVelocityHolonomic0::computeError()
   getVelocityHolonomicNormalized(cfg_, _vertices, a1, a2, a3);
 
   // error
-  _error[0] = penaltyBoundToInterval(std::hypot(std::hypot(a1,a2),a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[0] = penaltyBoundToInterval(error_0_1(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
 }
 
 void EdgeVelocityHolonomic1::computeError()
@@ -160,10 +161,10 @@ void EdgeVelocityHolonomic1::computeError()
   getVelocityHolonomicNormalized(cfg_, _vertices, a1, a2, a3);
 
   // error
-  _error[0] = penaltyBoundToInterval(a1+a2+a3, 1.0, cfg_->optim.penalty_epsilon);
-  _error[1] = penaltyBoundToInterval(a1+a2-a3, 1.0, cfg_->optim.penalty_epsilon);
-  _error[2] = penaltyBoundToInterval(a1-a2+a3, 1.0, cfg_->optim.penalty_epsilon);
-  _error[3] = penaltyBoundToInterval(a1-a2-a3, 1.0, cfg_->optim.penalty_epsilon);
+  _error[0] = penaltyBoundToInterval(error_1_1(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[1] = penaltyBoundToInterval(error_1_2(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[2] = penaltyBoundToInterval(error_1_3(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[3] = penaltyBoundToInterval(error_1_4(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
 }
 
 void EdgeVelocityHolonomic2::computeError()
@@ -173,10 +174,10 @@ void EdgeVelocityHolonomic2::computeError()
   getVelocityHolonomicNormalized(cfg_, _vertices, a1, a2, a3);
 
   // error
-  _error[0] = penaltyBoundToInterval(a1+a3, 1.0, cfg_->optim.penalty_epsilon);
-  _error[1] = penaltyBoundToInterval(a1-a3, 1.0, cfg_->optim.penalty_epsilon);
-  _error[2] = penaltyBoundToInterval(a2+a3, 1.0, cfg_->optim.penalty_epsilon);
-  _error[3] = penaltyBoundToInterval(a2-a3, 1.0, cfg_->optim.penalty_epsilon);
+  _error[0] = penaltyBoundToInterval(error_2_1(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[1] = penaltyBoundToInterval(error_2_2(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[2] = penaltyBoundToInterval(error_2_3(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[3] = penaltyBoundToInterval(error_2_4(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
 }
 
 void EdgeVelocityHolonomic3::computeError()
@@ -186,9 +187,9 @@ void EdgeVelocityHolonomic3::computeError()
   getVelocityHolonomicNormalized(cfg_, _vertices, a1, a2, a3);
 
   // error
-  _error[0] = penaltyBoundToInterval(a3+a2, 1.0, cfg_->optim.penalty_epsilon);
-  _error[1] = penaltyBoundToInterval(a3-a1-a2*0.5, 1.0, cfg_->optim.penalty_epsilon);
-  _error[2] = penaltyBoundToInterval(a3+a1-a2*0.5, 1.0, cfg_->optim.penalty_epsilon);
+  _error[0] = penaltyBoundToInterval(error_3_1(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[1] = penaltyBoundToInterval(error_3_2(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[2] = penaltyBoundToInterval(error_3_3(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
 }
 
 void EdgeVelocityHolonomic4::computeError()
@@ -198,9 +199,9 @@ void EdgeVelocityHolonomic4::computeError()
   getVelocityHolonomicNormalized(cfg_, _vertices, a1, a2, a3);
 
   // error
-  _error[0] = penaltyBoundToInterval(a3+a1, 1.0, cfg_->optim.penalty_epsilon);
-  _error[1] = penaltyBoundToInterval(a3-a1*0.5+a2, 1.0, cfg_->optim.penalty_epsilon);
-  _error[2] = penaltyBoundToInterval(a3-a1*0.5-a2, 1.0, cfg_->optim.penalty_epsilon);
+  _error[0] = penaltyBoundToInterval(error_4_1(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[1] = penaltyBoundToInterval(error_4_2(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[2] = penaltyBoundToInterval(error_4_3(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
 }
 
 void EdgeVelocityHolonomic5::computeError()
@@ -210,9 +211,9 @@ void EdgeVelocityHolonomic5::computeError()
   getVelocityHolonomicNormalized(cfg_, _vertices, a1, a2, a3);
 
   // error
-  _error[0] = penaltyBoundToInterval(a3-a2, 1.0, cfg_->optim.penalty_epsilon);
-  _error[1] = penaltyBoundToInterval(a3+a1+a2*0.5, 1.0, cfg_->optim.penalty_epsilon);
-  _error[2] = penaltyBoundToInterval(a3-a1+a2*0.5, 1.0, cfg_->optim.penalty_epsilon);
+  _error[0] = penaltyBoundToInterval(error_5_1(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[1] = penaltyBoundToInterval(error_5_2(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[2] = penaltyBoundToInterval(error_5_3(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
 }
 
 void EdgeVelocityHolonomic6::computeError()
@@ -222,9 +223,9 @@ void EdgeVelocityHolonomic6::computeError()
   getVelocityHolonomicNormalized(cfg_, _vertices, a1, a2, a3);
 
   // error
-  _error[0] = penaltyBoundToInterval(a3-a1, 1.0, cfg_->optim.penalty_epsilon);
-  _error[1] = penaltyBoundToInterval(a3+a1*0.5-a2, 1.0, cfg_->optim.penalty_epsilon);
-  _error[2] = penaltyBoundToInterval(a3+a1*0.5+a2, 1.0, cfg_->optim.penalty_epsilon);
+  _error[0] = penaltyBoundToInterval(error_6_1(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[1] = penaltyBoundToInterval(error_6_2(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
+  _error[2] = penaltyBoundToInterval(error_6_3(a1, a2, a3), 1.0, cfg_->optim.penalty_epsilon);
 }
 
 } // end namespace
diff --git a/src/optimal_planner.cpp b/src/optimal_planner.cpp
index 0b6b2bde..40d2ed0d 100644
--- a/src/optimal_planner.cpp
+++ b/src/optimal_planner.cpp
@@ -37,6 +37,7 @@
  *********************************************************************/
 
 #include <teb_local_planner/optimal_planner.h>
+#include <teb_local_planner/omni_helper.hpp>
 #include <map>
 #include <memory>
 #include <limits>
@@ -772,7 +773,7 @@ void TebOptimalPlanner::AddEdgesVelocity()
         {
           Eigen::Matrix<double,1,1> information;
           information.fill(0);
-          information(0,0) = std::hypot(std::hypot(cfg_->optim.weight_max_vel_x, cfg_->optim.weight_max_vel_y), cfg_->optim.weight_max_vel_theta);
+          information(0,0) = weight_0_1(cfg_->optim.weight_max_vel_x, cfg_->optim.weight_max_vel_y, cfg_->optim.weight_max_vel_theta);
 
           for (int i=0; i < n - 1; ++i)
           {
@@ -790,10 +791,10 @@ void TebOptimalPlanner::AddEdgesVelocity()
         {
           Eigen::Matrix<double,4,4> information;
           information.fill(0);
-          information(0,0) = cfg_->optim.weight_max_vel_x + cfg_->optim.weight_max_vel_y + cfg_->optim.weight_max_vel_theta;
-          information(1,1) = information(0,0);
-          information(2,2) = information(0,0);
-          information(3,3) = information(0,0);
+          information(0,0) = weight_1_1(cfg_->optim.weight_max_vel_x, cfg_->optim.weight_max_vel_y, cfg_->optim.weight_max_vel_theta);
+          information(1,1) = weight_1_2(cfg_->optim.weight_max_vel_x, cfg_->optim.weight_max_vel_y, cfg_->optim.weight_max_vel_theta);
+          information(2,2) = weight_1_3(cfg_->optim.weight_max_vel_x, cfg_->optim.weight_max_vel_y, cfg_->optim.weight_max_vel_theta);
+          information(3,3) = weight_1_4(cfg_->optim.weight_max_vel_x, cfg_->optim.weight_max_vel_y, cfg_->optim.weight_max_vel_theta);
 
           for (int i=0; i < n - 1; ++i)
           {
@@ -811,10 +812,10 @@ void TebOptimalPlanner::AddEdgesVelocity()
         {
           Eigen::Matrix<double,4,4> information;
           information.fill(0);
-          information(0,0) = cfg_->optim.weight_max_vel_x + cfg_->optim.weight_max_vel_theta;
-          information(1,1) = information(0,0);
-          information(2,2) = cfg_->optim.weight_max_vel_y + cfg_->optim.weight_max_vel_theta;
-          information(3,3) = information(2,2);
+          information(0,0) = weight_2_1(cfg_->optim.weight_max_vel_x, cfg_->optim.weight_max_vel_y, cfg_->optim.weight_max_vel_theta);
+          information(1,1) = weight_2_2(cfg_->optim.weight_max_vel_x, cfg_->optim.weight_max_vel_y, cfg_->optim.weight_max_vel_theta);
+          information(2,2) = weight_2_3(cfg_->optim.weight_max_vel_x, cfg_->optim.weight_max_vel_y, cfg_->optim.weight_max_vel_theta);
+          information(3,3) = weight_2_4(cfg_->optim.weight_max_vel_x, cfg_->optim.weight_max_vel_y, cfg_->optim.weight_max_vel_theta);
 
           for (int i=0; i < n - 1; ++i)
           {
@@ -832,9 +833,9 @@ void TebOptimalPlanner::AddEdgesVelocity()
         {
           Eigen::Matrix<double,3,3> information;
           information.fill(0);
-          information(0,0) = cfg_->optim.weight_max_vel_y + cfg_->optim.weight_max_vel_theta;
-          information(1,1) = cfg_->optim.weight_max_vel_x + cfg_->optim.weight_max_vel_y * 0.5 + cfg_->optim.weight_max_vel_theta;
-          information(2,2) = information(1,1);
+          information(0,0) = weight_3_1(cfg_->optim.weight_max_vel_x, cfg_->optim.weight_max_vel_y, cfg_->optim.weight_max_vel_theta);
+          information(1,1) = weight_3_2(cfg_->optim.weight_max_vel_x, cfg_->optim.weight_max_vel_y, cfg_->optim.weight_max_vel_theta);
+          information(2,2) = weight_3_3(cfg_->optim.weight_max_vel_x, cfg_->optim.weight_max_vel_y, cfg_->optim.weight_max_vel_theta);
 
           for (int i=0; i < n - 1; ++i)
           {
@@ -852,9 +853,9 @@ void TebOptimalPlanner::AddEdgesVelocity()
         {
           Eigen::Matrix<double,3,3> information;
           information.fill(0);
-          information(0,0) = cfg_->optim.weight_max_vel_x + cfg_->optim.weight_max_vel_theta;
-          information(1,1) = cfg_->optim.weight_max_vel_x * 0.5 + cfg_->optim.weight_max_vel_y + cfg_->optim.weight_max_vel_theta;
-          information(2,2) = information(1,1);
+          information(0,0) = weight_4_1(cfg_->optim.weight_max_vel_x, cfg_->optim.weight_max_vel_y, cfg_->optim.weight_max_vel_theta);
+          information(1,1) = weight_4_2(cfg_->optim.weight_max_vel_x, cfg_->optim.weight_max_vel_y, cfg_->optim.weight_max_vel_theta);
+          information(2,2) = weight_4_3(cfg_->optim.weight_max_vel_x, cfg_->optim.weight_max_vel_y, cfg_->optim.weight_max_vel_theta);
 
           for (int i=0; i < n - 1; ++i)
           {
@@ -872,9 +873,9 @@ void TebOptimalPlanner::AddEdgesVelocity()
         {
           Eigen::Matrix<double,3,3> information;
           information.fill(0);
-          information(0,0) = cfg_->optim.weight_max_vel_y + cfg_->optim.weight_max_vel_theta;
-          information(1,1) = cfg_->optim.weight_max_vel_x + cfg_->optim.weight_max_vel_y * 0.5 + cfg_->optim.weight_max_vel_theta;
-          information(2,2) = information(1,1);
+          information(0,0) = weight_5_1(cfg_->optim.weight_max_vel_x, cfg_->optim.weight_max_vel_y, cfg_->optim.weight_max_vel_theta);
+          information(1,1) = weight_5_2(cfg_->optim.weight_max_vel_x, cfg_->optim.weight_max_vel_y, cfg_->optim.weight_max_vel_theta);
+          information(2,2) = weight_5_3(cfg_->optim.weight_max_vel_x, cfg_->optim.weight_max_vel_y, cfg_->optim.weight_max_vel_theta);
 
           for (int i=0; i < n - 1; ++i)
           {
@@ -892,9 +893,9 @@ void TebOptimalPlanner::AddEdgesVelocity()
         {
           Eigen::Matrix<double,3,3> information;
           information.fill(0);
-          information(0,0) = cfg_->optim.weight_max_vel_x + cfg_->optim.weight_max_vel_theta;
-          information(1,1) = cfg_->optim.weight_max_vel_x * 0.5 + cfg_->optim.weight_max_vel_y + cfg_->optim.weight_max_vel_theta;
-          information(2,2) = information(1,1);
+          information(0,0) = weight_6_1(cfg_->optim.weight_max_vel_x, cfg_->optim.weight_max_vel_y, cfg_->optim.weight_max_vel_theta);
+          information(1,1) = weight_6_2(cfg_->optim.weight_max_vel_x, cfg_->optim.weight_max_vel_y, cfg_->optim.weight_max_vel_theta);
+          information(2,2) = weight_6_3(cfg_->optim.weight_max_vel_x, cfg_->optim.weight_max_vel_y, cfg_->optim.weight_max_vel_theta);
 
           for (int i=0; i < n - 1; ++i)
           {
@@ -993,7 +994,7 @@ void TebOptimalPlanner::AddEdgesAcceleration()
         {
           Eigen::Matrix<double,1,1> information;
           information.fill(0);
-          information(0,0) = std::hypot(std::hypot(cfg_->optim.weight_acc_lim_x, cfg_->optim.weight_acc_lim_y), cfg_->optim.weight_acc_lim_theta);
+          information(0,0) = weight_0_1(cfg_->optim.weight_acc_lim_x, cfg_->optim.weight_acc_lim_y, cfg_->optim.weight_acc_lim_theta);
 
           // check if an initial velocity should be taken into accound
           if (vel_start_.first)
@@ -1040,10 +1041,10 @@ void TebOptimalPlanner::AddEdgesAcceleration()
         {
           Eigen::Matrix<double,4,4> information;
           information.fill(0);
-          information(0,0) = cfg_->optim.weight_acc_lim_x + cfg_->optim.weight_acc_lim_y + cfg_->optim.weight_acc_lim_theta;
-          information(1,1) = information(0,0);
-          information(2,2) = information(0,0);
-          information(3,3) = information(0,0);
+          information(0,0) = weight_1_1(cfg_->optim.weight_acc_lim_x, cfg_->optim.weight_acc_lim_y, cfg_->optim.weight_acc_lim_theta);
+          information(1,1) = weight_1_2(cfg_->optim.weight_acc_lim_x, cfg_->optim.weight_acc_lim_y, cfg_->optim.weight_acc_lim_theta);
+          information(2,2) = weight_1_3(cfg_->optim.weight_acc_lim_x, cfg_->optim.weight_acc_lim_y, cfg_->optim.weight_acc_lim_theta);
+          information(3,3) = weight_1_4(cfg_->optim.weight_acc_lim_x, cfg_->optim.weight_acc_lim_y, cfg_->optim.weight_acc_lim_theta);
 
           // check if an initial velocity should be taken into accound
           if (vel_start_.first)
@@ -1090,10 +1091,10 @@ void TebOptimalPlanner::AddEdgesAcceleration()
         {
           Eigen::Matrix<double,4,4> information;
           information.fill(0);
-          information(0,0) = cfg_->optim.weight_acc_lim_x + cfg_->optim.weight_acc_lim_theta;
-          information(1,1) = information(0,0);
-          information(2,2) = cfg_->optim.weight_acc_lim_y + cfg_->optim.weight_acc_lim_theta;
-          information(3,3) = information(2,2);
+          information(0,0) = weight_2_1(cfg_->optim.weight_acc_lim_x, cfg_->optim.weight_acc_lim_y, cfg_->optim.weight_acc_lim_theta);
+          information(1,1) = weight_2_2(cfg_->optim.weight_acc_lim_x, cfg_->optim.weight_acc_lim_y, cfg_->optim.weight_acc_lim_theta);
+          information(2,2) = weight_2_3(cfg_->optim.weight_acc_lim_x, cfg_->optim.weight_acc_lim_y, cfg_->optim.weight_acc_lim_theta);
+          information(3,3) = weight_2_4(cfg_->optim.weight_acc_lim_x, cfg_->optim.weight_acc_lim_y, cfg_->optim.weight_acc_lim_theta);
 
           // check if an initial velocity should be taken into accound
           if (vel_start_.first)
@@ -1140,9 +1141,9 @@ void TebOptimalPlanner::AddEdgesAcceleration()
         {
           Eigen::Matrix<double,3,3> information;
           information.fill(0);
-          information(0,0) = cfg_->optim.weight_acc_lim_y + cfg_->optim.weight_acc_lim_theta;
-          information(1,1) = cfg_->optim.weight_acc_lim_x + cfg_->optim.weight_acc_lim_y * 0.5 + cfg_->optim.weight_acc_lim_theta;
-          information(2,2) = information(1,1);
+          information(0,0) = weight_3_1(cfg_->optim.weight_acc_lim_x, cfg_->optim.weight_acc_lim_y, cfg_->optim.weight_acc_lim_theta);
+          information(1,1) = weight_3_2(cfg_->optim.weight_acc_lim_x, cfg_->optim.weight_acc_lim_y, cfg_->optim.weight_acc_lim_theta);
+          information(2,2) = weight_3_3(cfg_->optim.weight_acc_lim_x, cfg_->optim.weight_acc_lim_y, cfg_->optim.weight_acc_lim_theta);
 
           // check if an initial velocity should be taken into accound
           if (vel_start_.first)
@@ -1189,9 +1190,9 @@ void TebOptimalPlanner::AddEdgesAcceleration()
         {
           Eigen::Matrix<double,3,3> information;
           information.fill(0);
-          information(0,0) = cfg_->optim.weight_acc_lim_x + cfg_->optim.weight_acc_lim_theta;
-          information(1,1) = cfg_->optim.weight_acc_lim_x * 0.5 + cfg_->optim.weight_acc_lim_y + cfg_->optim.weight_acc_lim_theta;
-          information(2,2) = information(1,1);
+          information(0,0) = weight_4_1(cfg_->optim.weight_acc_lim_x, cfg_->optim.weight_acc_lim_y, cfg_->optim.weight_acc_lim_theta);
+          information(1,1) = weight_4_2(cfg_->optim.weight_acc_lim_x, cfg_->optim.weight_acc_lim_y, cfg_->optim.weight_acc_lim_theta);
+          information(2,2) = weight_4_3(cfg_->optim.weight_acc_lim_x, cfg_->optim.weight_acc_lim_y, cfg_->optim.weight_acc_lim_theta);
 
           // check if an initial velocity should be taken into accound
           if (vel_start_.first)
@@ -1238,9 +1239,9 @@ void TebOptimalPlanner::AddEdgesAcceleration()
         {
           Eigen::Matrix<double,3,3> information;
           information.fill(0);
-          information(0,0) = cfg_->optim.weight_acc_lim_y + cfg_->optim.weight_acc_lim_theta;
-          information(1,1) = cfg_->optim.weight_acc_lim_x + cfg_->optim.weight_acc_lim_y * 0.5 + cfg_->optim.weight_acc_lim_theta;
-          information(2,2) = information(1,1);
+          information(0,0) = weight_5_1(cfg_->optim.weight_acc_lim_x, cfg_->optim.weight_acc_lim_y, cfg_->optim.weight_acc_lim_theta);
+          information(1,1) = weight_5_2(cfg_->optim.weight_acc_lim_x, cfg_->optim.weight_acc_lim_y, cfg_->optim.weight_acc_lim_theta);
+          information(2,2) = weight_5_3(cfg_->optim.weight_acc_lim_x, cfg_->optim.weight_acc_lim_y, cfg_->optim.weight_acc_lim_theta);
 
           // check if an initial velocity should be taken into accound
           if (vel_start_.first)
@@ -1287,9 +1288,9 @@ void TebOptimalPlanner::AddEdgesAcceleration()
         {
           Eigen::Matrix<double,3,3> information;
           information.fill(0);
-          information(0,0) = cfg_->optim.weight_acc_lim_x + cfg_->optim.weight_acc_lim_theta;
-          information(1,1) = cfg_->optim.weight_acc_lim_x * 0.5 + cfg_->optim.weight_acc_lim_y + cfg_->optim.weight_acc_lim_theta;
-          information(2,2) = information(1,1);
+          information(0,0) = weight_6_1(cfg_->optim.weight_acc_lim_x, cfg_->optim.weight_acc_lim_y, cfg_->optim.weight_acc_lim_theta);
+          information(1,1) = weight_6_2(cfg_->optim.weight_acc_lim_x, cfg_->optim.weight_acc_lim_y, cfg_->optim.weight_acc_lim_theta);
+          information(2,2) = weight_6_3(cfg_->optim.weight_acc_lim_x, cfg_->optim.weight_acc_lim_y, cfg_->optim.weight_acc_lim_theta);
 
           // check if an initial velocity should be taken into accound
           if (vel_start_.first)
diff --git a/src/teb_local_planner_ros.cpp b/src/teb_local_planner_ros.cpp
index 875a9947..d7a16442 100644
--- a/src/teb_local_planner_ros.cpp
+++ b/src/teb_local_planner_ros.cpp
@@ -37,6 +37,7 @@
  *********************************************************************/
 
 #include <teb_local_planner/teb_local_planner_ros.h>
+#include <teb_local_planner/omni_helper.hpp>
 
 #include <tf2_eigen/tf2_eigen.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
@@ -854,25 +855,25 @@ void TebLocalPlannerROS::saturateVelocityHolonomic(double& vx, double& vy, doubl
   double l = 0.0;
   switch (omni_type) {
     case 0:
-      l = std::hypot(std::hypot(a1, a2), a3);
+      l = limit_0(a1, a2, a3);
       break;
     case 1:
-      l = std::fabs(a1) + std::fabs(a2) + std::fabs(a3);
+      l = limit_1(a1, a2, a3);
       break;
     case 2:
-      l = std::max(std::fabs(a1), std::fabs(a2)) + std::fabs(a3);
+      l = limit_2(a1, a2, a3);
       break;
     case 3:
-      l = std::max(std::fabs(a3 + a2), std::max(std::fabs(a3 - a1 - a2 * 0.5), std::fabs(a3 + a1 - a2 * 0.5)));
+      l = limit_3(a1, a2, a3);
       break;
     case 4:
-      l = std::max(std::fabs(a3 + a1), std::max(std::fabs(a3 - a1 * 0.5 + a2), std::fabs(a3 - a1 * 0.5 - a2)));
+      l = limit_4(a1, a2, a3);
       break;
     case 5:
-      l = std::max(std::fabs(a3 - a2), std::max(std::fabs(a3 + a1 + a2 * 0.5), std::fabs(a3 - a1 + a2 * 0.5)));
+      l = limit_5(a1, a2, a3);
       break;
     case 6:
-      l = std::max(std::fabs(a3 - a1), std::max(std::fabs(a3 + a1 * 0.5 - a2), std::fabs(a3 + a1 * 0.5 + a2)));
+      l = limit_6(a1, a2, a3);
       break;
     default:
       l = 0.0;