Dynamics.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 
24 {
25  this->prevTime = -10.;
26  this->state = 0.;
27 }
28 
31  sdf::ElementPtr _sdf)
32 {
33  if (!_sdf->HasElement("type"))
34  {
35  std::cerr << "dynamics does not have a type element" << std::endl;
36  return NULL;
37  }
38 
39  std::string identifier = _sdf->Get<std::string>("type");
40 
41  if (creators_.find(identifier) == creators_.end())
42  {
43  std::cerr << "Cannot create ThrusterDynamics with unknown identifier: "
44  << identifier << std::endl;
45  return NULL;
46  }
47 
48  return creators_[identifier](_sdf);
49 }
50 
53 {
54  static DynamicsFactory instance;
55  return instance;
56 }
57 
59 bool DynamicsFactory::RegisterCreator(const std::string& _identifier,
60  DynamicsCreator _creator)
61 {
62  if (creators_.find(_identifier) != creators_.end())
63  {
64  std::cerr << "Warning: Registering ThrusterDynamics with identifier: "
65  << _identifier << " twice" << std::endl;
66  }
67  creators_[_identifier] = _creator;
68 
69  std::cout << "Registered ThrusterDynamics type " << _identifier << std::endl;
70  return true;
71 }
72 
74 const std::string DynamicsZeroOrder::IDENTIFIER = "ZeroOrder";
77 
78 Dynamics* DynamicsZeroOrder::create(sdf::ElementPtr _sdf)
80 {
81  return new DynamicsZeroOrder();
82 }
83 
85 double DynamicsZeroOrder::update(double _cmd, double _t)
86 {
87  return _cmd; // no dynamics
88 }
89 
91 const std::string DynamicsFirstOrder::IDENTIFIER = "FirstOrder";
94 
95 Dynamics* DynamicsFirstOrder::create(sdf::ElementPtr _sdf)
97 {
98  if (!_sdf->HasElement("timeConstant"))
99  {
100  std::cerr << "DynamicsFirstOrder: expected element time_constant"
101  << std::endl;
102  return NULL;
103  }
104  double tau = _sdf->Get<double>("timeConstant");
105  return new DynamicsFirstOrder(tau);
106 }
107 
109 double DynamicsFirstOrder::update(double _cmd, double _t)
110 {
111  if (prevTime < 0)
112  {
113  prevTime = _t;
114  return state;
115  }
116 
117  double dt = _t - prevTime;
118 
119  double alpha = std::exp(-dt/tau);
120  state = state*alpha + (1.0 - alpha)*_cmd;
121 
122  prevTime = _t;
123 
124  return state;
125 }
126 
129  : Dynamics(), tau(_tau)
130 {
131 }
132 
134 const std::string ThrusterDynamicsYoerger::IDENTIFIER = "Yoerger";
137 
138 Dynamics* ThrusterDynamicsYoerger::create(sdf::ElementPtr _sdf)
140 {
141  if (!_sdf->HasElement("alpha"))
142  {
143  std::cerr << "ThrusterDynamicsYoerger: expected element alpha"
144  << std::endl;
145  return NULL;
146  }
147  double alpha = _sdf->Get<double>("alpha");
148 
149  if (!_sdf->HasElement("beta"))
150  {
151  std::cerr << "ThrusterDynamicsYoerger: expected element beta"
152  << std::endl;
153  return NULL;
154  }
155  double beta = _sdf->Get<double>("beta");
156  return new ThrusterDynamicsYoerger(alpha, beta);
157 }
158 
160 double ThrusterDynamicsYoerger::update(double _cmd, double _t)
161 {
162  if (prevTime < 0)
163  {
164  prevTime = _t;
165  return state;
166  }
167 
168  double dt = _t - prevTime;
169 
170  state += dt*(beta*_cmd - alpha*state*std::abs(state));
171 
172  return state;
173 }
174 
177  : Dynamics(), alpha(_alpha), beta(_beta)
178 {
179 }
180 
181 
183 const std::string ThrusterDynamicsBessa::IDENTIFIER = "Bessa";
186 
187 Dynamics* ThrusterDynamicsBessa::create(sdf::ElementPtr _sdf)
189 {
190  if (!_sdf->HasElement("Jmsp"))
191  {
192  std::cerr << "ThrusterDynamicsBessa: expected element Jmsp"
193  << std::endl;
194  return NULL;
195  }
196 
197  if (!_sdf->HasElement("Kv1"))
198  {
199  std::cerr << "ThrusterDynamicsBessa: expected element Kv1"
200  << std::endl;
201  return NULL;
202  }
203 
204  if (!_sdf->HasElement("Kv2"))
205  {
206  std::cerr << "ThrusterDynamicsBessa: expected element Kv2"
207  << std::endl;
208  return NULL;
209  }
210 
211  if (!_sdf->HasElement("Kt"))
212  {
213  std::cerr << "ThrusterDynamicsBessa: expected element Kt"
214  << std::endl;
215  return NULL;
216  }
217 
218  if (!_sdf->HasElement("Rm"))
219  {
220  std::cerr << "ThrusterDynamicsBessa: expected element Rm"
221  << std::endl;
222  return NULL;
223  }
224  return new ThrusterDynamicsBessa(_sdf->Get<double>("Jmsp"),
225  _sdf->Get<double>("Kv1"),
226  _sdf->Get<double>("Kv2"),
227  _sdf->Get<double>("Kt"),
228  _sdf->Get<double>("Rm"));
229 }
230 
232 double ThrusterDynamicsBessa::update(double _cmd, double _t)
233 {
234  if (prevTime < 0)
235  {
236  prevTime = _t;
237  return state;
238  }
239 
240  double dt = _t - prevTime;
241 
242  state += dt*(_cmd*Kt/Rm - Kv1*state
243  - Kv2*state*std::abs(state))/Jmsp;
244 
245  return state;
246 }
247 
250  double _Kv2, double _Kt,
251  double _Rm) :
252  Dynamics(), Jmsp(_Jmsp), Kv1(_Kv1), Kv2(_Kv2), Kt(_Kt), Rm(_Rm)
253 {
254 }
255 }
virtual double update(double _cmd, double _t)
Update dynamical model given input value and time.
Definition: Dynamics.cc:160
1D dynamic models
ThrusterDynamicsBessa(double _Jmsp, double _Kv1, double _kv2, double _Kt, double _Rm)
Constructor.
Definition: Dynamics.cc:249
Yoerger&#39;s dynamic thruster model.
Definition: Dynamics.hh:143
Bessa&#39;s dynamic thruster model.
Definition: Dynamics.hh:176
Dynamics * CreateDynamics(sdf::ElementPtr _sdf)
Create ThrusterDynamics object according to its sdf Description.
Definition: Dynamics.cc:30
ThrusterDynamicsYoerger(double alpha, double beta)
Constructor.
Definition: Dynamics.cc:176
static const std::string IDENTIFIER
Unique identifier for this dynamical model.
Definition: Dynamics.hh:191
static DynamicsFactory & GetInstance()
Returns the singleton instance of this factory.
Definition: Dynamics.cc:52
virtual double update(double _cmd, double _t)
Update dynamical model given input value and time.
Definition: Dynamics.cc:109
virtual double update(double _cmd, double _t)
Update dynamical model given input value and time.
Definition: Dynamics.cc:85
Dynamics *(* DynamicsCreator)(sdf::ElementPtr)
Function pointer to create a certain thruster dynamics object.
Definition: Dynamics.hh:57
virtual void Reset()
Definition: Dynamics.cc:23
static const std::string IDENTIFIER
Unique identifier for this dynamical model.
Definition: Dynamics.hh:158
double prevTime
Time of last state update.
Definition: Dynamics.hh:50
static const std::string IDENTIFIER
Unique identifier for this dynamical model.
Definition: Dynamics.hh:128
static Dynamics * create(sdf::ElementPtr _sdf)
Create thruster model of this type with parameter values from sdf.
virtual double update(double _cmd, double _t)
Update dynamical model given input value and time.
Definition: Dynamics.cc:232
static Dynamics * create(sdf::ElementPtr _sdf)
Create thruster model of this type with parameter values from sdf.
Abstract base class for thruster dynamics.
Definition: Dynamics.hh:30
double state
Latest state.
Definition: Dynamics.hh:53
static Dynamics * create(sdf::ElementPtr _sdf)
Create thruster model of this type with parameter values from sdf.
bool RegisterCreator(const std::string &_identifier, DynamicsCreator _creator)
Register a ThrusterDynamic class with its creator.
Definition: Dynamics.cc:59
First-order dynamic system.
Definition: Dynamics.hh:113
static Dynamics * create(sdf::ElementPtr _sdf)
Create thruster model of this type with parameter values from sdf.
DynamicsFirstOrder(double _tau)
Constructor.
Definition: Dynamics.cc:128
static const std::string IDENTIFIER
Unique identifier for this dynamical model.
Definition: Dynamics.hh:105
REGISTER_DYNAMICS_CREATOR(DynamicsZeroOrder,&DynamicsZeroOrder::create) Dynamics *DynamicsZeroOrder
Definition: Dynamics.cc:75
Trivial (no dynamics) zero-order dynamic system.
Definition: Dynamics.hh:90
Factory singleton class that creates a ThrusterDynamics from sdf.
Definition: Dynamics.hh:60


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