Filter.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 
18 #ifndef _GAZEBO_FILTER_HH_
19 #define _GAZEBO_FILTER_HH_
20 
21 #include <gazebo/math/Vector3.hh>
23 
24 #ifndef _WIN32
25 #pragma GCC diagnostic push
26 #pragma GCC diagnostic ignored "-Wdeprecated-declarations"
27 #endif
28 
29 namespace gazebo
30 {
32 
34  namespace math
35  {
38 
42  template <class T>
43  class GZ_MATH_VISIBLE Filter
44  {
46  public: virtual ~Filter() GAZEBO_DEPRECATED(8.0) {}
47 
50  public: virtual void SetValue(const T &_val) GAZEBO_DEPRECATED(8.0)
51  { y0 = _val; }
52 
56  public: virtual void SetFc(double _fc, double _fs)
57  GAZEBO_DEPRECATED(8.0) = 0;
58 
61  public: inline virtual const T& GetValue() GAZEBO_DEPRECATED(8.0)
62  { return y0; }
63 
65  protected: T y0;
66  };
67 
71  template <class T>
72  class GZ_MATH_VISIBLE OnePole : public Filter<T>
73  {
76  public: OnePole() GAZEBO_DEPRECATED(8.0) : a0(0), b1(0) {}
77 
82  public: OnePole(double _fc, double _fs) GAZEBO_DEPRECATED(8.0)
83  : a0(0), b1(0)
84  {
85  this->SetFc(_fc, _fs);
86  }
87 
88  // Documentation Inherited.
89  public: virtual void SetFc(double _fc, double _fs)
90  {
91  b1 = exp(-2.0 * M_PI * _fc / _fs);
92  a0 = 1.0 - b1;
93  }
94 
98  public: inline const T& Process(const T &_x)
99  {
100  this->y0 = a0 * _x + b1 * this->y0;
101  return this->y0;
102  }
103 
105  protected: double a0;
106 
108  protected: double b1;
109  };
110 
113  class GZ_MATH_VISIBLE OnePoleQuaternion : public OnePole<math::Quaternion>
114  {
117  {
118  this->SetValue(math::Quaternion(1, 0, 0, 0));
119  }
120 
124  public: OnePoleQuaternion(double _fc, double _fs) GAZEBO_DEPRECATED(8.0)
125  : OnePole<math::Quaternion>(_fc, _fs)
126  {
127  this->SetValue(math::Quaternion(1, 0, 0, 0));
128  }
129 
133  public: inline const math::Quaternion& Process(const math::Quaternion &_x)
134  {
135  y0 = math::Quaternion::Slerp(a0, y0, _x);
136  return y0;
137  }
138  };
139 
142  class GZ_MATH_VISIBLE OnePoleVector3 : public OnePole<math::Vector3>
143  {
146  {
147  this->SetValue(math::Vector3(0, 0, 0));
148  }
149 
153  public: OnePoleVector3(double _fc, double _fs)
154  : OnePole<math::Vector3>(_fc, _fs)
155  {
156  this->SetValue(math::Vector3(0, 0, 0));
157  }
158  };
159 
163  template <class T>
164  class GZ_MATH_VISIBLE BiQuad : public Filter<T>
165  {
168  public: BiQuad() GAZEBO_DEPRECATED(8.0)
169  : a0(0), a1(0), a2(0), b0(0), b1(0), b2(0)
170  {
171  }
172 
177  public: BiQuad(double _fc, double _fs) GAZEBO_DEPRECATED(8.0)
178  : a0(0), a1(0), a2(0), b0(0), b1(0), b2(0)
179  {
180  this->SetFc(_fc, _fs);
181  }
182 
183  // Documentation Inherited.
184  public: inline void SetFc(double _fc, double _fs)
185  {
186  this->SetFc(_fc, _fs, 0.5);
187  }
188 
193  public: inline void SetFc(double _fc, double _fs, double _q)
194  {
195  double k = tan(M_PI * _fc / _fs);
196  double kQuadDenom = k * k + k / _q + 1.0;
197  this->a0 = k * k/ kQuadDenom;
198  this->a1 = 2 * this->a0;
199  this->a2 = this->a0;
200  this->b0 = 1.0;
201  this->b1 = 2 * (k * k - 1.0) / kQuadDenom;
202  this->b2 = (k * k - k / _q + 1.0) / kQuadDenom;
203  }
204 
207  public: virtual void SetValue(const T &_val)
208  {
209  this->y0 = this->y1 = this->y2 = this->x1 = this->x2 = _val;
210  }
211 
215  public: inline virtual const T& process(const T &_x)
216  {
217  this->y0 = this->a0 * _x +
218  this->a1 * this->x1 +
219  this->a2 * this->x2 -
220  this->b1 * this->y1 -
221  this->b2 * this->y2;
222 
223  this->x2 = this->x1;
224  this->x1 = _x;
225  this->y2 = this->y1;
226  this->y1 = this->y0;
227  return this->y0;
228  }
229 
231  protected: double a0, a1, a2, b0, b1, b2;
232 
234  protected: T x1, x2, y1, y2;
235  };
236 
239  class GZ_MATH_VISIBLE BiQuadVector3 : public BiQuad<math::Vector3>
240  {
243  {
244  this->SetValue(math::Vector3(0, 0, 0));
245  }
246 
250  public: BiQuadVector3(double _fc, double _fs) GAZEBO_DEPRECATED(8.0)
251  : BiQuad<math::Vector3>(_fc, _fs)
252  {
253  this->SetValue(math::Vector3(0, 0, 0));
254  }
255  };
256 
258  }
259 }
260 
261 #ifndef _WIN32
262 #pragma GCC diagnostic pop
263 #endif
264 #endif
BiQuadVector3() GAZEBO_DEPRECATED(8.0)
Constructor.
Definition: Filter.hh:242
void SetFc(double _fc, double _fs, double _q)
Set the cutoff frequency, sample rate and Q coefficient.
Definition: Filter.hh:193
virtual void SetValue(const T &_val)
Set the current filter&#39;s output.
Definition: Filter.hh:207
BiQuad vector3 filter.
Definition: Filter.hh:239
Forward declarations for the common classes.
Definition: Animation.hh:33
One-pole quaternion filter.
Definition: Filter.hh:113
const math::Quaternion & Process(const math::Quaternion &_x)
Update the filter&#39;s output.
Definition: Filter.hh:133
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:44
double a0
Input gain control.
Definition: Filter.hh:105
OnePole(double _fc, double _fs) GAZEBO_DEPRECATED(8.0)
Constructor.
Definition: Filter.hh:82
static const double GAZEBO_DEPRECATED(8.0) MAX_D
Double maximum value. This value will be similar to 1.79769e+308.
Definition: Helpers.hh:140
OnePoleVector3() GAZEBO_DEPRECATED(8.0)
Constructor.
Definition: Filter.hh:145
T y0
Output.
Definition: Filter.hh:65
static Quaternion Slerp(double _fT, const Quaternion &_rkP, const Quaternion &_rkQ, bool _shortestPath=false) GAZEBO_DEPRECATED(8.0)
Spherical linear interpolation between 2 quaternions, given the ends and an interpolation parameter b...
virtual void SetValue(const T &_val) GAZEBO_DEPRECATED(8.0)
Set the output of the filter.
Definition: Filter.hh:50
BiQuad(double _fc, double _fs) GAZEBO_DEPRECATED(8.0)
Constructor.
Definition: Filter.hh:177
T y2
Definition: Filter.hh:234
double b1
Gain of the feedback.
Definition: Filter.hh:108
virtual const T & GetValue() GAZEBO_DEPRECATED(8.0)
Get the output of the filter.
Definition: Filter.hh:61
Bi-quad filter base class.
Definition: Filter.hh:164
virtual void SetFc(double _fc, double _fs)
Set the cutoff frequency and sample rate.
Definition: Filter.hh:89
A one-pole DSP filter.
Definition: Filter.hh:72
One-pole vector3 filter.
Definition: Filter.hh:142
double b2
Definition: Filter.hh:231
void SetFc(double _fc, double _fs)
Set the cutoff frequency and sample rate.
Definition: Filter.hh:184
OnePoleQuaternion() GAZEBO_DEPRECATED(8.0)
Constructor.
Definition: Filter.hh:116
A quaternion class.
Definition: Quaternion.hh:48
BiQuad() GAZEBO_DEPRECATED(8.0)
Constructor.
Definition: Filter.hh:168
OnePoleQuaternion(double _fc, double _fs) GAZEBO_DEPRECATED(8.0)
Constructor.
Definition: Filter.hh:124
virtual const T & process(const T &_x)
Update the filter&#39;s output.
Definition: Filter.hh:215
Filter base class.
Definition: Filter.hh:43
BiQuadVector3(double _fc, double _fs) GAZEBO_DEPRECATED(8.0)
Constructor.
Definition: Filter.hh:250
OnePole() GAZEBO_DEPRECATED(8.0)
Constructor.
Definition: Filter.hh:76
OnePoleVector3(double _fc, double _fs)
Constructor.
Definition: Filter.hh:153
const T & Process(const T &_x)
Update the filter&#39;s output.
Definition: Filter.hh:98
virtual ~Filter() GAZEBO_DEPRECATED(8.0)
Destructor.
Definition: Filter.hh:46