LiftDragModel.cc
Go to the documentation of this file.
1 // Copyright (c) 2016 The UUV Simulator Authors.
2 // All rights reserved.
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 #include <gazebo/gazebo.hh>
17 
19 
20 namespace gazebo {
21 
23 bool LiftDrag::CheckForElement(sdf::ElementPtr _sdf,
24  const std::string &element)
25 {
26  if (!_sdf->HasElement(element))
27  {
28  std::cerr << " LiftDrag: Missing required element: "
29  << element
30  << std::endl;
31  return false;
32  }
33  return true;
34 }
35 
38  sdf::ElementPtr _sdf)
39 {
40  if (!_sdf->HasElement("type"))
41  {
42  std::cerr << "liftdrag does not have a type element" << std::endl;
43  return NULL;
44  }
45 
46  std::string identifier = _sdf->Get<std::string>("type");
47 
48  if (creators_.find(identifier) == creators_.end())
49  {
50  std::cerr << "Cannot create LiftDrag with unknown identifier: "
51  << identifier << std::endl;
52  return NULL;
53  }
54 
55  return creators_[identifier](_sdf);
56 }
57 
60 {
61  static LiftDragFactory instance;
62  return instance;
63 }
64 
66 bool LiftDragFactory::RegisterCreator(const std::string& _identifier,
67  LiftDragCreator _creator)
68 {
69  if (creators_.find(_identifier) != creators_.end())
70  {
71  std::cerr << "Warning: Registering LiftDrag with identifier: "
72  << _identifier << " twice" << std::endl;
73  }
74  creators_[_identifier] = _creator;
75 
76  std::cout << "Registered LiftDrag type " << _identifier << std::endl;
77  return true;
78 }
79 
81 const std::string LiftDragQuadratic::IDENTIFIER = "Quadratic";
84 
85 LiftDrag* LiftDragQuadratic::create(sdf::ElementPtr _sdf)
87 {
88  if (!_sdf->HasElement("lift_constant"))
89  {
90  std::cerr << "LiftDragQuadratic: expected element lift_constant"
91  << std::endl;
92  return NULL;
93  }
94 
95  if (!_sdf->HasElement("drag_constant"))
96  {
97  std::cerr << "LiftDragQuadratic: expected element drag_constant"
98  << std::endl;
99  return NULL;
100  }
101 
102  gzmsg << "Lift constant= " << _sdf->Get<double>("lift_constant") << std::endl;
103  gzmsg << "Drag constant= " << _sdf->Get<double>("drag_constant") << std::endl;
104 
105  return new LiftDragQuadratic(_sdf->Get<double>("lift_constant"),
106  _sdf->Get<double>("drag_constant"));
107 }
108 
110 ignition::math::Vector3d LiftDragQuadratic::compute(
111  const ignition::math::Vector3d &_velL)
112 {
113  ignition::math::Vector3d velL = _velL;
114  double angle = atan2(_velL.Y(), _velL.X());
115 
116  if (angle > M_PI_2)
117  {
118  angle -= M_PI;
119  velL = -_velL;
120  }
121  else if (angle < -M_PI_2)
122  {
123  angle += M_PI;
124  velL = -_velL;
125  }
126 
127  double u = velL.Length();
128  double u2 = u * u;
129  double du2 = angle * u2;
130 
131  double drag = angle * du2 * this->dragConstant;
132  double lift = du2 * this->liftConstant;
133 
134  ignition::math::Vector3d liftDirectionL =
135  -ignition::math::Vector3d::UnitZ.Cross(_velL).Normalize();
136  ignition::math::Vector3d dragDirectionL = -_velL;
137 
138  return lift*liftDirectionL + drag*dragDirectionL.Normalize();
139 }
140 
142 bool LiftDragQuadratic::GetParam(std::string _tag, double& _output)
143 {
144  _output = 0.0;
145  if (!_tag.compare("drag_constant"))
146  _output = this->dragConstant;
147  else if (!_tag.compare("lift_constant"))
148  _output = this->liftConstant;
149  else
150  return false;
151 
152  gzmsg << "LiftDragQuadratic::GetParam <" << _tag << ">=" << _output <<
153  std::endl;
154  return true;
155 }
156 
158 std::map<std::string, double> LiftDragQuadratic::GetListParams()
159 {
160  std::map<std::string, double> params;
161  params["drag_constant"] = this->dragConstant;
162  params["lift_constant"] = this->liftConstant;
163  return params;
164 }
165 
167 const std::string LiftDragTwoLines::IDENTIFIER = "TwoLines";
170 
171 LiftDrag* LiftDragTwoLines::create(sdf::ElementPtr _sdf)
173 {
174  if (LiftDrag::CheckForElement(_sdf, "area") &&
175  LiftDrag::CheckForElement(_sdf, "fluid_density") &&
176  LiftDrag::CheckForElement(_sdf, "a0") &&
177  LiftDrag::CheckForElement(_sdf, "alpha_stall") &&
178  LiftDrag::CheckForElement(_sdf, "cla") &&
179  LiftDrag::CheckForElement(_sdf, "cla_stall") &&
180  LiftDrag::CheckForElement(_sdf, "cda") &&
181  LiftDrag::CheckForElement(_sdf, "cda_stall"))
182  {
183  return new LiftDragTwoLines(_sdf->Get<double>("area"),
184  _sdf->Get<double>("fluid_density"),
185  _sdf->Get<double>("a0"),
186  _sdf->Get<double>("alpha_stall"),
187  _sdf->Get<double>("cla"),
188  _sdf->Get<double>("cla_stall"),
189  _sdf->Get<double>("cda"),
190  _sdf->Get<double>("cda_stall"));
191  }
192  else
193  return NULL;
194 }
195 
197 ignition::math::Vector3d LiftDragTwoLines::compute(const ignition::math::Vector3d &_velL)
198 {
199  // The following computations are based on the LiftDragPlugin included
200  // in Gazebo simulator 7.0 (http://gazebosim.org)
201  // Copyright (C) 2012-2016 Open Source Robotics Foundation
202  // This source code is licensed under the Apache-2.0 License found in
203  // the open_source_licenses.txt file in the root directory of this source
204  // tree.
205  ignition::math::Vector3d velL = _velL;
206  double angle = atan2(_velL.Y(), _velL.X());
207 
208  // Make sure angle is in [-pi/2, pi/2]
209  if (angle > M_PI_2)
210  {
211  angle -= M_PI;
212  velL = -_velL;
213  }
214  else if (angle < -M_PI_2)
215  {
216  angle += M_PI;
217  velL = -_velL;
218  }
219 
220  double alpha = angle + this->a0;
221 
222  // Again, normalize to within +/-90 deg
223  while (fabs(alpha) > 0.5 * M_PI)
224  {
225  alpha = alpha > 0 ? alpha - M_PI : alpha + M_PI;
226  }
227 
228  double u = velL.Length();
229 
230  // Compute dynamic pressure:
231  double q = 0.5 * this->fluidDensity * u * u;
232 
233  // Compute lift&drag coefficients:
234  double cl, cd;
235  if (alpha > this->alphaStall)
236  {
237  // stall
238  double diffAlpha = alpha - this->alphaStall;
239  cl = this->cla*this->alphaStall +
240  this->claStall * diffAlpha;
241  cd = this->cda*this->alphaStall +
242  this->cdaStall * diffAlpha;
243  }
244  else if (alpha < -this->alphaStall)
245  {
246  // stall
247  double sumAlpha = alpha + this->alphaStall;
248  cl = -this->cla * this->alphaStall +
249  this->cdaStall * sumAlpha;
250  cd = -this->cda * this->alphaStall +
251  this->cdaStall * sumAlpha;
252  }
253  else
254  {
255  // no stall
256  cd = this->cda * alpha;
257  cl = this->cla * alpha;
258  }
259 
260  double lift = cl*q*this->area;
261  double drag = cd*q*this->area;
262 
263  ignition::math::Vector3d liftDirectionL = -ignition::math::Vector3d::UnitZ.Cross(_velL).Normalize();
264  ignition::math::Vector3d dragDirectionL = -_velL;
265 
266  return lift*liftDirectionL + drag*dragDirectionL.Normalize();
267 }
268 
270 bool LiftDragTwoLines::GetParam(std::string _tag, double& _output)
271 {
272  _output = 0.0;
273  if (!_tag.compare("area"))
274  _output = this->area;
275  else if (!_tag.compare("fluid_density"))
276  _output = this->fluidDensity;
277  else if (!_tag.compare("a0"))
278  _output = this->a0;
279  else if (!_tag.compare("alpha_stall"))
280  _output = this->alphaStall;
281  else if (!_tag.compare("cla"))
282  _output = this->cla;
283  else if (!_tag.compare("cla_stall"))
284  _output = this->claStall;
285  else if (!_tag.compare("cda"))
286  _output = this->cda;
287  else if (!_tag.compare("cda_stall"))
288  _output = this->cdaStall;
289  else
290  return false;
291 
292  gzmsg << "LiftDragQuadratic::GetParam <" << _tag << ">=" << _output <<
293  std::endl;
294  return true;
295 }
296 
298 std::map<std::string, double> LiftDragTwoLines::GetListParams()
299 {
300  std::map<std::string, double> params;
301  params["area"] = this->area;
302  params["fluid_density"] = this->fluidDensity;
303  params["a0"] = this->a0;
304  params["alpha_stall"] = this->alphaStall;
305  params["cla"] = this->cla;
306  params["cla_stall"] = this->claStall;
307  params["cda"] = this->cda;
308  params["cda_stall"] = this->cdaStall;
309  return params;
310 }
311 
312 }
virtual bool GetParam(std::string _tag, double &_output)
Return paramater in scalar form for the given tag.
LiftDrag * CreateLiftDrag(sdf::ElementPtr _sdf)
Create LiftDrag object according to its sdf Description.
static LiftDrag * create(sdf::ElementPtr _sdf)
Create thruster model of this type with parameter values from sdf.
virtual bool GetParam(std::string _tag, double &_output)
Return paramater in scalar form for the given tag.
static const std::string IDENTIFIER
Unique identifier for this dynamical model.
Lift&drag model that models lift/drag coeffs using two lines. This is based on Gazebo&#39;s LiftDragPlugin bu...
Abstract base class for Lift&Drag models.
virtual std::map< std::string, double > GetListParams()
Return list of all parameters.
Various Lift&Drag models for Fins.
REGISTER_LIFTDRAG_CREATOR(LiftDragQuadratic,&LiftDragQuadratic::create) LiftDrag *LiftDragQuadratic
bool RegisterCreator(const std::string &_identifier, LiftDragCreator _creator)
Register a LiftDrag class with its creator.
static LiftDrag * create(sdf::ElementPtr _sdf)
Create thruster model of this type with parameter values from sdf.
LiftDrag *(* LiftDragCreator)(sdf::ElementPtr)
Function pointer to create a certain LiftDrag object.
virtual ignition::math::Vector3d compute(const ignition::math::Vector3d &_velL)
Compute the lift and drag force.
static LiftDragFactory & GetInstance()
Returns the singleton instance of this factory.
virtual ignition::math::Vector3d compute(const ignition::math::Vector3d &velL)
Compute the lift and drag force.
static const std::string IDENTIFIER
Unique identifier for this dynamical model.
Basic quadratic (Hugin) lift&drag model, page 18 from [1]. [1] Engelhardtsen, Øystein. "3D AUV Collision Avoidance." (2007).
Factory singleton class that creates a LiftDrag from sdf.
static bool CheckForElement(sdf::ElementPtr _sdf, const std::string &element)
Check for element. Complain and return 0 if it is missing.
virtual std::map< std::string, double > GetListParams()
Return list of all parameters.


uuv_gazebo_plugins
Author(s): Musa Morena Marcusso Manhaes , Sebastian Scherer , Luiz Ricardo Douat
autogenerated on Thu Jun 18 2020 03:28:24