ThrusterConversionFcn.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 
17 
19 
20 namespace gazebo
21 {
24  sdf::ElementPtr _sdf)
25 {
26  if (!_sdf->HasElement("type"))
27  {
28  std::cerr << "conversion does not have a type element" << std::endl;
29  return NULL;
30  }
31 
32  std::string identifier = _sdf->Get<std::string>("type");
33 
34  if (creators_.find(identifier) == creators_.end())
35  {
36  std::cerr << "Cannot creatae ConversionFunction with unknown identifier: "
37  << identifier << std::endl;
38  return NULL;
39  }
40 
41  return creators_[identifier](_sdf);
42 }
43 
46 {
47  static ConversionFunctionFactory instance;
48  return instance;
49 }
50 
52 bool ConversionFunctionFactory::RegisterCreator(const std::string& _identifier,
54 {
55  if (creators_.find(_identifier) != creators_.end())
56  {
57  std::cerr << "Warning: Registering ConversionFunction with identifier: "
58  << _identifier << " twice" << std::endl;
59  }
60  creators_[_identifier] = _creator;
61 
62  std::cout << "Registered ConversionFunction type "
63  << _identifier << std::endl;
64  return true;
65 }
66 
67 
69 const std::string ConversionFunctionBasic::IDENTIFIER = "Basic";
72 
73 ConversionFunction* ConversionFunctionBasic::create(sdf::ElementPtr _sdf)
75 {
76  if (!_sdf->HasElement("rotorConstant"))
77  {
78  std::cerr << "ConversionFunctionBasic::Expected element rotorConstant"
79  << std::endl;
80  return NULL;
81  }
82  double gain = _sdf->Get<double>("rotorConstant");
83  return new ConversionFunctionBasic(gain);
84 }
85 
88 {
89  return this->rotorConstant*std::abs(_cmd)*_cmd;
90 }
91 
93 bool ConversionFunctionBasic::GetParam(std::string _tag, double& _output)
94 {
95  _output = 0.0;
96  if (!_tag.compare("rotor_constant"))
97  _output = this->rotorConstant;
98  else
99  return false;
100 
101  gzmsg << "ConversionFunctionBasic::GetParam <" << _tag << ">=" << _output <<
102  std::endl;
103  return true;
104 }
105 
108  : rotorConstant(_rotorConstant)
109 {
110  gzmsg << "ConversionFunctionBasic::Create conversion function"
111  << std::endl
112  << "\t- rotorConstant: " << this->rotorConstant << std::endl;
113 }
114 
115 
117 const std::string ConversionFunctionBessa::IDENTIFIER = "Bessa";
120 
121 ConversionFunction* ConversionFunctionBessa::create(sdf::ElementPtr _sdf)
123 {
124  if (!_sdf->HasElement("rotorConstantL"))
125  {
126  std::cerr << "ConversionFunctionBasic: expected element rotorConstantL"
127  << std::endl;
128  return NULL;
129  }
130 
131  if (!_sdf->HasElement("rotorConstantR"))
132  {
133  std::cerr << "ConversionFunctionBasic::Expected element rotorConstantR"
134  << std::endl;
135  return NULL;
136  }
137 
138  if (!_sdf->HasElement("deltaL"))
139  {
140  std::cerr << "ConversionFunctionBasic::Expected element deltaL"
141  << std::endl;
142  return NULL;
143  }
144 
145  if (!_sdf->HasElement("deltaR"))
146  {
147  std::cerr << "ConversionFunctionBasic::Expected element deltaR"
148  << std::endl;
149  return NULL;
150  }
151 
152  return new ConversionFunctionBessa(_sdf->Get<double>("rotorConstantL"),
153  _sdf->Get<double>("rotorConstantR"),
154  _sdf->Get<double>("deltaL"),
155  _sdf->Get<double>("deltaR"));
156 }
157 
160 {
161  double basic = _cmd*std::abs(_cmd);
162 
163  if (basic <= this->deltaL)
164  {
165  return this->rotorConstantL*(basic - this->deltaL);
166  }
167  else if (basic >= this->deltaR)
168  {
169  return this->rotorConstantR*(basic - this->deltaR);
170  }
171  else
172  {
173  return 0;
174  }
175 }
176 
179  double _rotorConstantR,
180  double _deltaL,
181  double _deltaR) :
182  rotorConstantL(_rotorConstantL),
183  rotorConstantR(_rotorConstantR),
184  deltaL(_deltaL),
185  deltaR(_deltaR)
186 {
187  GZ_ASSERT(rotorConstantL >= 0.0,
188  "ConversionFunctionBessa: rotorConstantL should be >= 0");
189  GZ_ASSERT(rotorConstantR >= 0.0,
190  "ConversionFunctionBessa: rotorConstantR should be >= 0");
191  GZ_ASSERT(deltaL <= 0.0,
192  "ConversionFunctionBessa: deltaL should be <= 0");
193  GZ_ASSERT(deltaR >= 0.0,
194  "ConversionFunctionBessa: deltaR should be >= 0");
195 
196  gzmsg << "ConversionFunctionBessa:" << std::endl
197  << "\t- rotorConstantL: " << this->rotorConstantL << std::endl
198  << "\t- rotorConstantR: " << this->rotorConstantR << std::endl
199  << "\t- deltaL: " << this->deltaL << std::endl
200  << "\t- deltaR: " << this->deltaR << std::endl;
201 }
202 
204 bool ConversionFunctionBessa::GetParam(std::string _tag, double& _output)
205 {
206  _output = 0.0;
207  if (!_tag.compare("rotor_constant_l"))
208  _output = this->rotorConstantL;
209  else if (!_tag.compare("rotor_constant_r"))
210  _output = this->rotorConstantR;
211  else if (!_tag.compare("delta_l"))
212  _output = this->deltaL;
213  else if (!_tag.compare("delta_r"))
214  _output = this->deltaR;
215  else
216  return false;
217 
218  gzmsg << "ConversionFunctionBessa::GetParam <" << _tag << ">=" << _output <<
219  std::endl;
220  return true;
221 }
222 
224 const std::string ConversionFunctionLinearInterp::IDENTIFIER = "LinearInterp";
227 
230 {
231  if (!_sdf->HasElement("inputValues"))
232  {
233  std::cerr << "ConversionFunctionLinearInterp::Expected element inputValues"
234  << std::endl;
235  return NULL;
236  }
237 
238  if (!_sdf->HasElement("outputValues"))
239  {
240  std::cerr << "ConversionFunctionLinearInterp::Expected element outputValues"
241  << std::endl;
242  return NULL;
243  }
244 
245  std::vector<double> in = Str2Vector(_sdf->Get<std::string>("inputValues"));
246  std::vector<double> out = Str2Vector(_sdf->Get<std::string>("outputValues"));
247 
248  if (in.size() < 1)
249  {
250  std::cerr << "ConversionFunctionLinearInterp::"
251  << "Need at least one input/output pair"
252  << std::endl;
253  return NULL;
254  }
255 
256  if (in.size() != out.size())
257  {
258  std::cerr << "ConversionFunctionLinearInterp::"
259  << "Number of input and output values should be the same"
260  << std::endl;
261  return NULL;
262  }
263 
264  return new ConversionFunctionLinearInterp(in, out);
265 }
266 
269 {
270  GZ_ASSERT(!lookupTable.empty(), "Lookup table is empty");
271 
272  // "first element whose key is NOT considered to go before _cmd"
273  auto iter = lookupTable.lower_bound(_cmd);
274 
275  if (iter == lookupTable.end())
276  {
277  // "all keys are considered to go before"
278  // last element is closest
279  return lookupTable.rbegin()->second;
280  }
281 
282  double i1 = iter->first;
283  double o1 = iter->second;
284 
285  if (iter == lookupTable.begin())
286  return o1;
287 
288  iter--;
289 
290  double i0 = iter->first;
291  double o0 = iter->second;
292 
293  double w1 = _cmd - i0;
294  double w0 = i1 - _cmd;
295 
296  return (o0*w0 + o1*w1)/(w0 + w1);
297 }
298 
301  const std::vector<double> &_input,
302  const std::vector<double> &_output)
303 {
304  GZ_ASSERT(_input.size() == _output.size(), "input and output do not match");
305 
306  for (int i = 0; i < _input.size(); i++)
307  {
308  lookupTable[_input[i]] = _output[i];
309  }
310  gzmsg << "ConversionFunctionLinearInterp::Create conversion function"
311  << std::endl;
312  gzmsg << "\t- Input values:" << std::endl;
313  for (auto& i : lookupTable)
314  std::cout << i.first << " ";
315  std::cout << std::endl;
316  gzmsg << "\t- Output values:" << std::endl;
317  for (auto& i : lookupTable)
318  std::cout << i.second << " ";
319  std::cout << std::endl;
320 }
321 
323 bool ConversionFunctionLinearInterp::GetParam(std::string _tag, double& _output)
324 {
325  return false;
326 }
327 
330 {
331  return this->lookupTable;
332 }
333 
334 }
ConversionFunctionLinearInterp(const std::vector< double > &_input, const std::vector< double > &_output)
Constructor.
virtual bool GetParam(std::string _tag, double &_output)
Return paramater in scalar form for the given tag.
virtual double convert(double _cmd)
Convert thruster state (e.g. angular velocity) to thrust force.
std::map< std::string, ConversionFunctionCreator > creators_
Map of each registered identifiers to its corresponding creator.
ConversionFunctionBessa(double _rotorConstantL, double _rotorConstantR, double _deltaL, double _deltaR)
Constructor.
ConversionFunction *(* ConversionFunctionCreator)(sdf::ElementPtr)
Function pointer to create a certain conversion function.
static const std::string IDENTIFIER
The unique identifier of this conversion function.
virtual bool GetParam(std::string _tag, double &_output)
Return paramater in scalar form for the given tag.
Asymmetric conversion function with dead-zone nonlinearity. This corresponds to what is called Model ...
virtual bool GetParam(std::string _tag, double &_output)
Return paramater in scalar form for the given tag.
static ConversionFunction * create(sdf::ElementPtr _sdf)
Create a ConversionFunction object according to its sdf Description.
Factory singleton class that creates a ConversionFunction from sdf.
Description of the conversion function fo a thruster.
static const std::string IDENTIFIER
The unique identifier of this conversion function.
Abstact base class for a thruster conversion function.
The most basic conversion function: Thrust = const.*w*abs(w) This corresponds to what is attrributed ...
double deltaR
Dead-zone for omega > 0.
ConversionFunction * CreateConversionFunction(sdf::ElementPtr _sdf)
Create a ConversionFunction object according to its sdf Description.
static ConversionFunction * create(sdf::ElementPtr _sdf)
Create a ConversionFunction object according to its sdf Description.
static ConversionFunction * create(sdf::ElementPtr _sdf)
Create a ConversionFunction object according to its sdf Description.
double rotorConstantR
Rotor constant for omega > 0.
std::vector< double > Str2Vector(std::string _input)
Conversion of a string to a double vector.
Definition: Def.hh:55
bool RegisterCreator(const std::string &_identifier, ConversionFunctionCreator _creator)
Register a ConversionFunction class with its creator.
double rotorConstantL
Rotor constant for omega < 0.
virtual double convert(double _cmd)
Convert thruster state (e.g. angular velocity) to thrust force.
static ConversionFunctionFactory & GetInstance()
Return the singleton instance of this factory.
virtual std::map< double, double > GetTable()
Return input and output vectors of the lookup table.
REGISTER_CONVERSIONFUNCTION_CREATOR(ConversionFunctionBasic,&ConversionFunctionBasic::create) ConversionFunction *ConversionFunctionBasic
General definitions.
static const std::string IDENTIFIER
The unique identifier of this conversion function.
virtual double convert(double _cmd)
Convert thruster state (e.g. angular velocity) to thrust force.
double deltaL
Dead-zone for omega < 0.
Conversion using linear interpolation between given data points.
ConversionFunctionBasic(double _rotorConstant)
Constructor.


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