ThrusterPlugin.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 <boost/algorithm/string.hpp>
17 #include <boost/bind.hpp>
18 #include <boost/shared_ptr.hpp>
19 
20 #include <limits>
21 
22 #include <gazebo/gazebo.hh>
23 #include <gazebo/msgs/msgs.hh>
24 #include <gazebo/physics/Link.hh>
25 #include <gazebo/physics/Model.hh>
26 #include <gazebo/physics/PhysicsEngine.hh>
27 #include <gazebo/physics/World.hh>
28 #include <gazebo/transport/TransportTypes.hh>
29 #include <sdf/sdf.hh>
30 
31 #include <math.h>
32 
35 
36 
38 
39 namespace gazebo {
40 
42 ThrusterPlugin::ThrusterPlugin() : thrustForce(0),
43  inputCommand(0),
44  clampMin(std::numeric_limits<double>::lowest()),
45  clampMax(std::numeric_limits<double>::max()),
46  thrustMin(std::numeric_limits<double>::lowest()),
47  thrustMax(std::numeric_limits<double>::max()),
48  gain(1.0),
49  isOn(true),
50  thrustEfficiency(1.0),
51  propellerEfficiency(1.0),
52  thrusterID(-1)
53 {
54 }
55 
58 {
59  if (this->updateConnection)
60  {
61 #if GAZEBO_MAJOR_VERSION >= 8
62  this->updateConnection.reset();
63 #else
64  event::Events::DisconnectWorldUpdateBegin(this->updateConnection);
65 #endif
66  }
67 }
68 
70 void ThrusterPlugin::Load(physics::ModelPtr _model,
71  sdf::ElementPtr _sdf)
72 {
73  GZ_ASSERT(_model != NULL, "Invalid model pointer");
74 
75  // Initializing the transport node
76  this->node = transport::NodePtr(new transport::Node());
77 #if GAZEBO_MAJOR_VERSION >= 8
78  this->node->Init(_model->GetWorld()->Name());
79 #else
80  this->node->Init(_model->GetWorld()->GetName());
81 #endif
82 
83  // Retrieve the link name on which the thrust will be applied
84  GZ_ASSERT(_sdf->HasElement("linkName"), "Could not find linkName.");
85  std::string linkName = _sdf->Get<std::string>("linkName");
86  this->thrusterLink = _model->GetLink(linkName);
87  GZ_ASSERT(this->thrusterLink, "thruster link is invalid");
88 
89  // Reading thruster ID
90  GZ_ASSERT(_sdf->HasElement("thrusterID"), "Thruster ID was not provided");
91  this->thrusterID = _sdf->Get<int>("thrusterID");
92 
93  // Thruster dynamics configuration:
94  GZ_ASSERT(_sdf->HasElement("dynamics"), "Could not find dynamics.");
95  this->thrusterDynamics.reset(
96  DynamicsFactory::GetInstance().CreateDynamics(
97  _sdf->GetElement("dynamics")));
98 
99  // Thrust conversion function
100  GZ_ASSERT(_sdf->HasElement("conversion"), "Could not find dynamics");
101  this->conversionFunction.reset(
102  ConversionFunctionFactory::GetInstance().CreateConversionFunction(
103  _sdf->GetElement("conversion")));
104 
105  // Optional paramters:
106  // Rotor joint, used for visualization if available.
107  if (_sdf->HasElement("jointName"))
108  this->joint = _model->GetJoint(_sdf->Get<std::string>("jointName"));
109 
110  // Clamping interval
111  if (_sdf->HasElement("clampMin"))
112  this->clampMin = _sdf->Get<double>("clampMin");
113 
114  if (_sdf->HasElement("clampMax"))
115  this->clampMax = _sdf->Get<double>("clampMax");
116 
117  if (this->clampMin >= this->clampMax)
118  {
119  gzmsg << "clampMax must be greater than clampMin, returning to default values..." << std::endl;
120  this->clampMin = std::numeric_limits<double>::lowest();
121  this->clampMax = std::numeric_limits<double>::max();
122  }
123 
124  // Thrust force interval
125  if (_sdf->HasElement("thrustMin"))
126  this->thrustMin = _sdf->Get<double>("thrustMin");
127 
128  if (_sdf->HasElement("thrustMax"))
129  this->thrustMax = _sdf->Get<double>("thrustMax");
130 
131  if (this->thrustMin >= this->thrustMax)
132  {
133  gzmsg << "thrustMax must be greater than thrustMin, returning to default values..." << std::endl;
134  this->thrustMin = std::numeric_limits<double>::lowest();
135  this->thrustMax = std::numeric_limits<double>::max();
136  }
137 
138  // Gain (1.0 by default)
139  if (_sdf->HasElement("gain"))
140  this->gain = _sdf->Get<double>("gain");
141 
142  if (_sdf->HasElement("thrust_efficiency"))
143  {
144  this->thrustEfficiency = _sdf->Get<double>("thrust_efficiency");
145  if (this->thrustEfficiency < 0.0 || this->thrustEfficiency > 1.0)
146  {
147  gzmsg << "Invalid thrust efficiency factor, setting it to 100%"
148  << std::endl;
149  this->thrustEfficiency = 1.0;
150  }
151  }
152 
153  if (_sdf->HasElement("propeller_efficiency"))
154  {
155  this->propellerEfficiency = _sdf->Get<double>("propeller_efficiency");
156  if (this->propellerEfficiency < 0.0 || this->propellerEfficiency > 1.0)
157  {
158  gzmsg <<
159  "Invalid propeller dynamics efficiency factor, setting it to 100%"
160  << std::endl;
161  this->propellerEfficiency = 1.0;
162  }
163  }
164  // Root string for topics
165  std::stringstream strs;
166  strs << "/" << _model->GetName() << "/thrusters/" << this->thrusterID << "/";
167  this->topicPrefix = strs.str();
168 
169  // Advertise the thrust topic
170  this->thrustTopicPublisher =
171  this->node->Advertise<msgs::Vector3d>(this->topicPrefix + "thrust");
172 
173  // Subscribe to the input signal topic
174 
175  this->commandSubscriber =
176  this->node->Subscribe(this->topicPrefix + "input",
178  this);
179 
180  // Connect the update event
181  this->updateConnection = event::Events::ConnectWorldUpdateBegin(
182  boost::bind(&ThrusterPlugin::Update,
183  this, _1));
184 #if GAZEBO_MAJOR_VERSION >= 8
185  this->thrusterAxis = this->joint->WorldPose().Rot().RotateVectorReverse(this->joint->GlobalAxis(0));
186 #else
187  this->thrusterAxis = this->joint->GetWorldPose().rot.Ign().RotateVectorReverse(this->joint->GetGlobalAxis(0).Ign());
188 #endif
189 }
190 
193 {
194 }
195 
198 {
199  this->thrusterDynamics->Reset();
200 }
201 
203 void ThrusterPlugin::Update(const common::UpdateInfo &_info)
204 {
205  GZ_ASSERT(!std::isnan(this->inputCommand),
206  "nan in this->inputCommand");
207 
208  double dynamicsInput;
209  double dynamicState;
210  // Test if the thruster has been turned off
211  if (this->isOn)
212  {
213  double clamped = this->inputCommand;
214  clamped = std::min(clamped, this->clampMax);
215  clamped = std::max(clamped, this->clampMin);
216 
217  dynamicsInput = this->gain*clamped;
218  }
219  else
220  {
221  // In case the thruster is turned off in runtime, the dynamic state
222  // will converge to zero
223  dynamicsInput = 0.0;
224  }
225  dynamicState = this->propellerEfficiency *
226  this->thrusterDynamics->update(dynamicsInput, _info.simTime.Double());
227 
228  GZ_ASSERT(!std::isnan(dynamicState), "Invalid dynamic state");
229  // Multiply the output force magnitude with the efficiency
230  this->thrustForce = this->thrustEfficiency *
231  this->conversionFunction->convert(dynamicState);
232  GZ_ASSERT(!std::isnan(this->thrustForce), "Invalid thrust force");
233 
234  // Use the thrust force limits
235  this->thrustForce = std::max(this->thrustForce, this->thrustMin);
236  this->thrustForce = std::min(this->thrustForce, this->thrustMax);
237 
238  this->thrustForceStamp = _info.simTime;
239  ignition::math::Vector3d force(this->thrustForce*this->thrusterAxis);
240 
241  this->thrusterLink->AddRelativeForce(force);
242 
243  if (this->joint)
244  {
245  // Let joint rotate with correct angular velocity.
246  this->joint->SetVelocity(0, dynamicState);
247  }
248 
249  // Publish thrust:
250  msgs::Vector3d thrustMsg;
251  msgs::Set(&thrustMsg, ignition::math::Vector3d(this->thrustForce, 0., 0.));
252  this->thrustTopicPublisher->Publish(thrustMsg);
253 }
254 
257 {
258  this->inputCommand = _msg->value();
259 }
260 }
void Update(const common::UpdateInfo &_info)
Update the simulation state.
virtual ~ThrusterPlugin()
Destructor.
double clampMax
: Optional: Commands greater than this value will be clamped.
physics::LinkPtr thrusterLink
Pointer to the thruster link.
physics::JointPtr joint
Optional: The rotor joint, used for visualization.
bool isOn
Optional: Flag to indicate if the thruster is turned on or off.
Model plugin for description of the thruster dynamics.
double propellerEfficiency
Optional: Propeller angular velocity efficiency term.
transport::PublisherPtr thrustTopicPublisher
Publisher to the output thrust topic.
const boost::shared_ptr< const uuv_gazebo_plugins_msgs::msgs::Double > ConstDoublePtr
Definition of a pointer to the floating point message.
Definition: FinPlugin.hh:36
std::shared_ptr< ConversionFunction > conversionFunction
Thruster conversion function.
GZ_REGISTER_MODEL_PLUGIN(UmbilicalPlugin)
common::Time thrustForceStamp
Time stamp of latest thrust force.
double clampMin
: Optional: Commands less than this value will be clamped.
double inputCommand
Input command, typically desired angular velocity of the rotor.
std::shared_ptr< Dynamics > thrusterDynamics
Thruster dynamic model.
static DynamicsFactory & GetInstance()
Returns the singleton instance of this factory.
Definition: Dynamics.cc:52
virtual void Reset()
Custom plugin reset behavior.
double thrustMax
: Optional: Maximum thrust force output
transport::SubscriberPtr commandSubscriber
Subscriber to the reference signal topic.
transport::NodePtr node
Gazebo node.
double thrustMin
: Optional: Minimum thrust force output
int thrusterID
Thruster ID, used to generated topic names automatically.
double thrustEfficiency
Optional: Output thrust efficiency factor of the thruster.
Class for the thruster plugin.
ignition::math::Vector3d thrusterAxis
The axis about which the thruster rotates.
double gain
: Optional: Gain factor: Desired angular velocity = command * gain
void UpdateInput(ConstDoublePtr &_msg)
Callback for the input topic subscriber.
virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
event::ConnectionPtr updateConnection
Update event.
std::string topicPrefix
Thruster topics prefix.
static ConversionFunctionFactory & GetInstance()
Return the singleton instance of this factory.
General definitions.
double thrustForce
Latest thrust force in [N].


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