penalties.h
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2016,
6  * TU Dortmund - Institute of Control Theory and Systems Engineering.
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the institute nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * Author: Christoph Rösmann
37  *********************************************************************/
38 
39 #ifndef PENALTIES_H
40 #define PENALTIES_H
41 
42 #include <cmath>
43 #include <Eigen/Core>
44 #include <g2o/stuff/misc.h>
45 
46 namespace teb_local_planner
47 {
48 
57 inline double penaltyBoundToInterval(const double& var,const double& a,const double& epsilon)
58 {
59  if (var < -a+epsilon)
60  {
61  return (-var - (a - epsilon));
62  }
63  if (var <= a-epsilon)
64  {
65  return 0.;
66  }
67  else
68  {
69  return (var - (a - epsilon));
70  }
71 }
72 
82 inline double penaltyBoundToInterval(const double& var,const double& a, const double& b, const double& epsilon)
83 {
84  if (var < a+epsilon)
85  {
86  return (-var + (a + epsilon));
87  }
88  if (var <= b-epsilon)
89  {
90  return 0.;
91  }
92  else
93  {
94  return (var - (b - epsilon));
95  }
96 }
97 
98 
107 inline double penaltyBoundFromBelow(const double& var, const double& a,const double& epsilon)
108 {
109  if (var >= a+epsilon)
110  {
111  return 0.;
112  }
113  else
114  {
115  return (-var + (a+epsilon));
116  }
117 }
118 
127 inline double penaltyBoundToIntervalDerivative(const double& var,const double& a, const double& epsilon)
128 {
129  if (var < -a+epsilon)
130  {
131  return -1;
132  }
133  if (var <= a-epsilon)
134  {
135  return 0.;
136  }
137  else
138  {
139  return 1;
140  }
141 }
142 
152 inline double penaltyBoundToIntervalDerivative(const double& var,const double& a, const double& b, const double& epsilon)
153 {
154  if (var < a+epsilon)
155  {
156  return -1;
157  }
158  if (var <= b-epsilon)
159  {
160  return 0.;
161  }
162  else
163  {
164  return 1;
165  }
166 }
167 
168 
177 inline double penaltyBoundFromBelowDerivative(const double& var, const double& a,const double& epsilon)
178 {
179  if (var >= a+epsilon)
180  {
181  return 0.;
182  }
183  else
184  {
185  return -1;
186  }
187 }
188 
189 
190 } // namespace teb_local_planner
191 
192 
193 #endif // PENALTIES_H
double penaltyBoundFromBelow(const double &var, const double &a, const double &epsilon)
Linear penalty function for bounding var from below: .
Definition: penalties.h:107
double penaltyBoundToInterval(const double &var, const double &a, const double &epsilon)
Linear penalty function for bounding var to the interval .
Definition: penalties.h:57
double penaltyBoundToIntervalDerivative(const double &var, const double &a, const double &epsilon)
Derivative of the linear penalty function for bounding var to the interval .
Definition: penalties.h:127
double penaltyBoundFromBelowDerivative(const double &var, const double &a, const double &epsilon)
Derivative of the linear penalty function for bounding var from below: .
Definition: penalties.h:177


teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Wed Jun 5 2019 19:25:10