thruster.py
Go to the documentation of this file.
1 # Copyright (c) 2016-2019 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 import rospy
16 import numpy
17 from tf_quaternion import transformations
18 from uuv_gazebo_ros_plugins_msgs.msg import FloatStamped
19 from std_msgs.msg import Header
20 
21 
22 class Thruster(object):
23  """Abstract function to all the thruster models avaialble.
24  The instance of a thruster model must use the factory method.
25 
26  > *Input arguments*
27 
28  * `index` (*type:* `int`): Thruster's ID.
29  * `topic` (*type:* `str`): Thruster's command topic.
30  * `pos` (*type:* `numpy.array` or `list`): Position vector
31  of the thruster with respect to the vehicle's frame.
32  * `orientation` (*type:* `numpy.array` or `list`): Quaternion
33  with the orientation of the thruster with respect to the vehicle's
34  frame as `(qx, qy, qz, qw)`.
35  * `axis` (*type:* `numpy.array`): Axis of rotation of the thruster.
36  """
37  LABEL = ''
38  DEFAULT_AXIS=numpy.array([1, 0, 0, 0])
39 
40  def __init__(self, index, topic, pos, orientation, axis=DEFAULT_AXIS):
41  self._index = index
42  self._topic = topic
43  self._pos = None
44  self._orientation = None
45  self._force_dist = None
46  if pos is not None and orientation is not None:
47  self._pos = pos
48  self._orientation = orientation
49  # compute contribution to configuration matrix of this thruster
50  thrust_body = transformations.quaternion_matrix(orientation).dot(
51  axis.transpose())[0:3]
52  torque_body = numpy.cross(pos, thrust_body)
53  self._force_dist = numpy.hstack((
54  thrust_body, torque_body)).transpose()
55  self._command = 0
56  self._thrust = 0
57  self._command_pub = rospy.Publisher(self._topic, FloatStamped,
58  queue_size=10)
59 
60  rospy.loginfo('Thruster #{} - {} - {}'.format(
61  self._index, self.LABEL, self._topic))
62 
63  @property
64  def index(self):
65  return self._index
66 
67  @property
68  def topic(self):
69  return self._topic
70 
71  @property
72  def tam_column(self):
73  return self._force_dist
74 
75  @staticmethod
76  def create_thruster(model_name, *args, **kwargs):
77  """Factory method for the thruster models.
78 
79  > *Input arguments*
80 
81  * `model_name` (*type:* `str`): Name identifier of
82  the thruster model.
83 
84  > *Returns*
85 
86  Thruster model instance.
87  """
88  for thruster in Thruster.__subclasses__():
89  if model_name == thruster.LABEL:
90  return thruster(*args, **kwargs)
91  raise rospy.ROSException('Invalid thruster model')
92 
93  def get_command_value(self, thrust):
94  """Convert desired thrust force to input command according to this
95  function. Overwrite this method to implement custom models."""
96  raise NotImplementedError()
97 
98  def get_thrust_value(self, command):
99  """Computes the thrust force for the given command."""
100  raise NotImplementedError()
101 
102  def get_curve(self, min_value, max_value, n_points):
103  """Sample the conversion curve and return the values."""
104  if min_value >= max_value or n_points <= 0:
105  return [], []
106  input_values = numpy.linspace(min_value, max_value, n_points)
107  output_values = []
108  for value in input_values:
109  output_values.append(self.get_thrust_value(value))
110  return input_values.tolist(), output_values
111 
112  def _calc_command(self):
113  """Convert the desired thrust force into angular velocity
114  command according using a gain."""
115  self._command = self.get_command_value(self._thrust)
116 
117  def _update(self, thrust):
118  self._thrust = thrust
119  self._calc_command()
120 
121  def publish_command(self, thrust):
122  """Publish the thrust force command set-point
123  to a thruster unit.
124 
125  > *Input arguments*
126 
127  * `thrust` (*type:* `float`): Thrust force set-point
128  in N.
129  """
130  self._update(thrust)
131  output = FloatStamped()
132  output.header.stamp = rospy.Time.now()
133  output.data = self._command
134  self._command_pub.publish(output)
def get_curve(self, min_value, max_value, n_points)
Definition: thruster.py:102
def create_thruster(model_name, args, kwargs)
Definition: thruster.py:76
def get_thrust_value(self, command)
Definition: thruster.py:98
def __init__(self, index, topic, pos, orientation, axis=DEFAULT_AXIS)
Definition: thruster.py:40
def get_command_value(self, thrust)
Definition: thruster.py:93


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