def odometry_callback(self, msg):
"""Handle updated measured velocity callback."""
if not bool(self.config):
return
linear = msg.twist.twist.linear
angular = msg.twist.twist.angular
v_linear = numpy.array([linear.x, linear.y, linear.z])
v_angular = numpy.array([angular.x, angular.y, angular.z])
if self.config['odom_vel_in_world']:
# This is a temp. workaround for gazebo's pos3d plugin not behaving properly:
# Twist should be provided wrt child_frame, gazebo provides it wrt world frame
# see http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html
xyzw_array = lambda o: numpy.array([o.x, o.y, o.z, o.w])
q_wb = xyzw_array(msg.pose.pose.orientation)
R_bw = transf.quaternion_matrix(q_wb)[0:3, 0:3].transpose()
v_linear = R_bw.dot(v_linear)
v_angular = R_bw.dot(v_angular)
# Compute compute control output:
t = time_in_float_sec_from_msg(msg.header.stamp)
e_v_linear = (self.v_linear_des - v_linear)
e_v_angular = (self.v_angular_des - v_angular)
a_linear = self.pid_linear.regulate(e_v_linear, t)
a_angular = self.pid_angular.regulate(e_v_angular, t)
# Convert and publish accel. command:
cmd_accel = geometry_msgs.Accel()
cmd_accel.linear = geometry_msgs.Vector3(x=a_linear[0], y=a_linear[1], z=a_linear[2])
cmd_accel.angular = geometry_msgs.Vector3(x=a_angular[0], y=a_angular[1], z=a_angular[2])
self.pub_cmd_accel.publish(cmd_accel)
def __init__(self, node: Node, index, topic, pos, orientation, axis=None):
self.node = node
self._index = index
self._topic = topic
self._pos = None
self._orientation = None
self._force_dist = None
if pos is not None and orientation is not None:
self._pos = pos
self._orientation = orientation
if axis is None:
axis = self.DEFAULT_AXIS
# compute contribution to configuration matrix of this thruster
thrust_body = transformations.quaternion_matrix(orientation).dot(
axis.transpose())[0:3]
torque_body = numpy.cross(pos, thrust_body)
self._force_dist = numpy.hstack((
thrust_body, torque_body)).transpose()
self._command = 0
self._thrust = 0
self._command_pub = self.node.create_publisher(FloatStamped, self._topic, 10)
self.node.get_logger().info('Thruster #{} - {} - {}'.format(
self._index, self.LABEL, self._topic))
def odometry_callback(self, msg):
"""Handle updated measured velocity callback."""
if not bool(self.config):
return
p = msg.pose.pose.position
q = msg.pose.pose.orientation
p = numpy.array([p.x, p.y, p.z])
q = numpy.array([q.x, q.y, q.z, q.w])
if not self.initialized:
# If this is the first callback: Store and hold latest pose.
self.pos_des = p
self.quat_des = q
self.initialized = True
# Compute control output:
t = time_in_float_sec_from_msg(msg.header.stamp)
# Position error
e_pos_world = self.pos_des - p
e_pos_body = transf.quaternion_matrix(q).transpose()[0:3, 0:3].dot(
e_pos_world)
# Error quaternion wrt body frame
e_rot_quat = transf.quaternion_multiply(transf.quaternion_conjugate(q),
self.quat_des)
if numpy.linalg.norm(e_pos_world[0:2]) > 5.0:
# special case if we are far away from goal:
# ignore desired heading, look towards goal position
heading = math.atan2(e_pos_world[1], e_pos_world[0])
quat_des = numpy.array(
[0, 0, math.sin(0.5 * heading),
math.cos(0.5 * heading)])
e_rot_quat = transf.quaternion_multiply(
transf.quaternion_conjugate(q), quat_des)
# Error angles
e_rot = numpy.array(transf.euler_from_quaternion(e_rot_quat))
v_linear = self.pid_pos.regulate(e_pos_body, t)
v_angular = self.pid_rot.regulate(e_rot, t)
# Convert and publish vel. command:
cmd_vel = geometry_msgs.Twist()
cmd_vel.linear = geometry_msgs.Vector3(x=v_linear[0],
y=v_linear[1],
z=v_linear[2])
cmd_vel.angular = geometry_msgs.Vector3(x=v_angular[0],
y=v_angular[1],
z=v_angular[2])
self.pub_cmd_vel.publish(cmd_vel)
def __init__(self, index, pos, quat, topic, node:rclpy.node.Node):
self.node = node
self.id = index
self.pos = pos
self.quat = quat
self.topic = topic
self.rot = quaternion_matrix(quat)[0:3, 0:3]
unit_z = self.rot[:, 2]
# Surge velocity wrt vehicle's body frame
surge_vel = np.array([1, 0, 0])
fin_surge_vel = surge_vel - np.dot(surge_vel, unit_z) / np.linalg.norm(unit_z)**2 * unit_z
# Compute the lift and drag vectors
self.lift_vector = -1 * np.cross(unit_z, fin_surge_vel) / np.linalg.norm(np.cross(unit_z, fin_surge_vel))
self.drag_vector = -1 * surge_vel / np.linalg.norm(surge_vel)
self.pub = self.node.create_publisher(FloatStamped, self.topic, 3)
def __init__(self, index, topic, pos, orientation, axis=DEFAULT_AXIS):
self._index = index
self._topic = topic
self._pos = None
self._orientation = None
self._force_dist = None
if pos is not None and orientation is not None:
self._pos = pos
self._orientation = orientation
# compute contribution to configuration matrix of this thruster
thrust_body = transformations.quaternion_matrix(orientation).dot(
axis.transpose())[0:3]
torque_body = numpy.cross(pos, thrust_body)
self._force_dist = numpy.hstack(
(thrust_body, torque_body)).transpose()
self._command = 0
self._thrust = 0
self._command_pub = rospy.Publisher(self._topic,
FloatStamped,
queue_size=10)
rospy.loginfo('Thruster #{} - {} - {}'.format(self._index, self.LABEL,
self._topic))
def __init__(self, node_name, **kwargs):
super().__init__(node_name,
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True,
**kwargs)
# Acquiring the namespace of the vehicle
self.namespace = self.get_namespace().replace('/', '')
self.get_logger().info(
'Initialize control allocator for vehicle <%s>' % self.namespace)
self.tf_buffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tf_buffer, self)
tf_trans_ned_to_enu = None
try:
if self.namespace != '':
target = '{}/base_link'.format(self.namespace)
source = '{}/base_link_ned'.format(self.namespace)
else:
target = 'base_link'
source = 'base_link_ned'
self.get_logger().info('Lookup transfrom from %s to %s' %
(source, target))
tf_trans_ned_to_enu = self.tf_buffer.lookup_transform(
).lookup_transform(target, source, rclpy.time.Time(),
rclpy.time.Duration(seconds=1))
except Exception as e:
self.get_logger().warning(
'No transform found between base_link and base_link_ned'
' for vehicle {}, message={}'.format(self.namespace, e))
self.base_link_ned_to_enu = None
if tf_trans_ned_to_enu is not None:
self.base_link_ned_to_enu = quaternion_matrix(
(tf_trans_ned_to_enu.transform.rotation.x,
tf_trans_ned_to_enu.transform.rotation.y,
tf_trans_ned_to_enu.transform.rotation.z,
tf_trans_ned_to_enu.transform.rotation.w))[0:3, 0:3]
self.get_logger().warning(
'base_link transform NED to ENU=\n{}'.format(
self.base_link_ned_to_enu))
self.base_link = self.get_parameter(
'base_link', 'base_link').get_parameter_value().string_value
# Retrieve the thruster configuration parameters if available
thruster_config = self.get_parameters_by_prefix('thruster_config')
if len(thruster_config) == 0:
raise RuntimeError('Thruster configuration not available')
self.thruster_config = parse_nested_params_to_dict(
self.thruster_config, '.', True)
# Check if all necessary thruster model parameter are available
thruster_params = [
'conversion_fcn_params', 'conversion_fcn', 'topic_prefix',
'topic_suffix', 'frame_base', 'max_thrust'
]
for p in thruster_params:
if p not in self.thruster_config:
raise RuntimeError(
'Parameter <%s> for thruster conversion function is missing'
% p)
# Setting up the thruster topic name
self.thruster_topic = build_topic_name(
self.namespace, self.thruster_config['topic_prefix'], 0,
self.thruster_config['topic_suffix'])
self.thruster = None
# Retrieve the fin configuration if available
fin_config = self.get_parameters_by_prefix('fin_config')
if len(fin_config) == 0:
raise RuntimeError('Fin configuration is not available')
self.fin_config = parse_nested_params_to_dict(self.fin_config, '.',
True)
# Check if all necessary fin parameters are available
fin_params = [
'fluid_density', 'lift_coefficient', 'fin_area', 'topic_prefix',
'topic_suffix', 'frame_base'
]
for p in fin_params:
if p not in self.fin_config:
raise RuntimeError(
'Parameter <%s> for fin configuration is missing' % p)
self.fin_lower_limit = -np.pi / 2
if 'lower_limit' in self.fin_config:
self.fin_lower_limit = self.fin_config['lower_limit']
self.fin_upper_limit = np.pi / 2
if 'upper_limit' in self.fin_config:
self.fin_upper_limit = self.fin_config['upper_limit']
if self.fin_config['lower_limit'] >= self.fin_config['upper_limit']:
raise RuntimeError('Fin angle limits are invalid')
self.fins = dict()
self.n_fins = 0
if not self.find_actuators():
raise RuntimeError('No thruster and/or fins found')
def get_force_vector(pos, orientation, axis):
thrust_body = transformations.quaternion_matrix(orientation).dot(
axis.transpose())[0:3]
torque_body = np.cross(pos, thrust_body)
return np.hstack((thrust_body, torque_body)).transpose()
def __init__(self, inertial_frame_id='world'):
"""Class constructor."""
assert inertial_frame_id in ['world', 'world_ned']
# Reading current namespace
self._namespace = rospy.get_namespace()
self._inertial_frame_id = inertial_frame_id
self._body_frame_id = None
self._logger = get_logger()
if self._inertial_frame_id == 'world':
self._body_frame_id = 'base_link'
else:
self._body_frame_id = 'base_link_ned'
try:
import tf2_ros
tf_buffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tf_buffer)
tf_trans_ned_to_enu = tf_buffer.lookup_transform(
'world', 'world_ned', rospy.Time(),
rospy.Duration(10))
self.q_ned_to_enu = np.array(
[tf_trans_ned_to_enu.transform.rotation.x,
tf_trans_ned_to_enu.transform.rotation.y,
tf_trans_ned_to_enu.transform.rotation.z,
tf_trans_ned_to_enu.transform.rotation.w])
except Exception as ex:
self._logger.warning(
'Error while requesting ENU to NED transform'
', message={}'.format(ex))
self.q_ned_to_enu = quaternion_from_euler(2 * np.pi, 0, np.pi)
self.transform_ned_to_enu = quaternion_matrix(
self.q_ned_to_enu)[0:3, 0:3]
if self.transform_ned_to_enu is not None:
self._logger.info('Transform world_ned (NED) to world (ENU)=\n' +
str(self.transform_ned_to_enu))
self._mass = 0
if rospy.has_param('~mass'):
self._mass = rospy.get_param('~mass')
if self._mass <= 0:
raise rospy.ROSException('Mass has to be positive')
self._inertial = dict(ixx=0, iyy=0, izz=0, ixy=0, ixz=0, iyz=0)
if rospy.has_param('~inertial'):
inertial = rospy.get_param('~inertial')
for key in self._inertial:
if key not in inertial:
raise rospy.ROSException('Invalid moments of inertia')
self._inertial = inertial
self._cog = [0, 0, 0]
if rospy.has_param('~cog'):
self._cog = rospy.get_param('~cog')
if len(self._cog) != 3:
raise rospy.ROSException('Invalid center of gravity vector')
self._cob = [0, 0, 0]
if rospy.has_param('~cog'):
self._cob = rospy.get_param('~cob')
if len(self._cob) != 3:
raise rospy.ROSException('Invalid center of buoyancy vector')
self._body_frame = 'base_link'
if rospy.has_param('~base_link'):
self._body_frame = rospy.get_param('~base_link')
self._volume = 0.0
if rospy.has_param('~volume'):
self._volume = rospy.get_param('~volume')
if self._volume <= 0:
raise rospy.ROSException('Invalid volume')
# Fluid density
self._density = 1028.0
if rospy.has_param('~density'):
self._density = rospy.get_param('~density')
if self._density <= 0:
raise rospy.ROSException('Invalid fluid density')
# Bounding box
self._height = 0.0
self._length = 0.0
self._width = 0.0
if rospy.has_param('~height'):
self._height = rospy.get_param('~height')
if self._height <= 0:
raise rospy.ROSException('Invalid height')
if rospy.has_param('~length'):
self._length = rospy.get_param('~length')
if self._length <= 0:
raise rospy.ROSException('Invalid length')
if rospy.has_param('~width'):
self._width = rospy.get_param('~width')
if self._width <= 0:
raise rospy.ROSException('Invalid width')
# Calculating the rigid-body mass matrix
self._M = np.zeros(shape=(6, 6), dtype=float)
self._M[0:3, 0:3] = self._mass * np.eye(3)
self._M[0:3, 3:6] = - self._mass * \
cross_product_operator(self._cog)
self._M[3:6, 0:3] = self._mass * \
cross_product_operator(self._cog)
self._M[3:6, 3:6] = self._calc_inertial_tensor()
# Loading the added-mass matrix
self._Ma = np.zeros((6, 6))
if rospy.has_param('~Ma'):
self._Ma = np.array(rospy.get_param('~Ma'))
if self._Ma.shape != (6, 6):
raise rospy.ROSException('Invalid added mass matrix')
# Sum rigid-body and added-mass matrices
self._Mtotal = np.zeros(shape=(6, 6))
self._calc_mass_matrix()
# Acceleration of gravity
self._gravity = 9.81
# Initialize the Coriolis and centripetal matrix
self._C = np.zeros((6, 6))
# Vector of restoring forces
self._g = np.zeros(6)
# Loading the linear damping coefficients
self._linear_damping = np.zeros(shape=(6, 6))
if rospy.has_param('~linear_damping'):
self._linear_damping = np.array(rospy.get_param('~linear_damping'))
if self._linear_damping.shape == (6,):
self._linear_damping = np.diag(self._linear_damping)
if self._linear_damping.shape != (6, 6):
raise rospy.ROSException('Linear damping must be given as a 6x6 matrix or the diagonal coefficients')
# Loading the nonlinear damping coefficients
self._quad_damping = np.zeros(shape=(6,))
if rospy.has_param('~quad_damping'):
self._quad_damping = np.array(rospy.get_param('~quad_damping'))
if self._quad_damping.shape != (6,):
raise rospy.ROSException('Quadratic damping must be given defined with 6 coefficients')
# Loading the linear damping coefficients proportional to the forward speed
self._linear_damping_forward_speed = np.zeros(shape=(6, 6))
if rospy.has_param('~linear_damping_forward_speed'):
self._linear_damping_forward_speed = np.array(
rospy.get_param('~linear_damping_forward_speed'))
if self._linear_damping_forward_speed.shape == (6,):
self._linear_damping_forward_speed = np.diag(self._linear_damping_forward_speed)
if self._linear_damping_forward_speed.shape != (6, 6):
raise rospy.ROSException(
'Linear damping proportional to the '
'forward speed must be given as a 6x6 '
'matrix or the diagonal coefficients')
# Initialize damping matrix
self._D = np.zeros((6, 6))
# Vehicle states
self._pose = dict(pos=np.zeros(3),
rot=quaternion_from_euler(0, 0, 0))
# Velocity in the body frame
self._vel = np.zeros(6)
# Acceleration in the body frame
self._acc = np.zeros(6)
# Generalized forces
self._gen_forces = np.zeros(6)
def _init_async_impl(self):
tf_trans_ned_to_enu = None
try:
if self.namespace != '':
target = '{}base_link'.format(self.namespace)
target = target[1::]
source = '{}base_link_ned'.format(self.namespace)
else:
target = 'base_link'
source = 'base_link_ned'
source = source[1::]
tf_trans_ned_to_enu = self.tf_buffer.lookup_transform(
target, source, rclpy.time.Time(),
rclpy.time.Duration(seconds=1))
except Exception as e:
self.get_logger().warn(
'No transform found between base_link and base_link_ned'
' for vehicle {}, message={}'.format(self.namespace, e))
self.base_link_ned_to_enu = None
if tf_trans_ned_to_enu is not None:
self.base_link_ned_to_enu = transformations.quaternion_matrix(
(tf_trans_ned_to_enu.transform.rotation.x,
tf_trans_ned_to_enu.transform.rotation.y,
tf_trans_ned_to_enu.transform.rotation.z,
tf_trans_ned_to_enu.transform.rotation.w))[0:3, 0:3]
self.get_logger().info('base_link transform NED to ENU=\n' +
str(self.base_link_ned_to_enu))
self.get_logger().info('ThrusterManager::update_rate=' +
str(self.config['update_rate'].value))
# Set the tf_prefix parameter
#TODO probably comment
#self.set_parameters(['thruster_manager/tf_prefix'], [self.namespace])
# param_tf_prefix = rclpy.parameter.Parameter('thruster_manager.tf_prefix', rclpy.Parameter.Type.STRING, self.namespace)
# self.set_parameters([param_tf_prefix])
# Retrieve the output file path to store the TAM
# matrix for future use
self.output_dir = None
if self.has_parameter('output_dir'):
self.output_dir = self.get_parameter(
'output_dir').get_parameter_value().string_value
if not isdir(self.output_dir):
raise RuntimeError('Invalid output directory, output_dir=' +
self.output_dir)
self.get_logger().info('output_dir=' + self.output_dir)
# Number of thrusters
self.n_thrusters = 0
# Thruster objects used to calculate the right angular velocity command
self.thrusters = list()
# Thrust forces vector
self.thrust = None
#TODO Close check...transform to dict
# Thruster allocation matrix: transform thruster inputs to force/torque
self.configuration_matrix = None
# tam = self.get_parameter('tam')
# if len(tam) != 0:
#tam = parse_nested_params_to_dict(tam, '.')
if self.has_parameter('tam'):
tam = self.get_parameter('tam').value
tam = numpy.array(tam)
#reshape the array. #TODO Unsure
self.configuration_matrix = numpy.reshape(tam, (6, -1))
# Set number of thrusters from the number of columns
self.n_thrusters = self.configuration_matrix.shape[1]
# Create publishing topics to each thruster
params = self.config['conversion_fcn_params']
conv_fcn = self.config['conversion_fcn'].value
if type(params) == list and type(conv_fcn) == list:
if len(params) != self.n_thrusters or len(
conv_fcn) != self.n_thrusters:
raise RuntimeError('Lists conversion_fcn and '
'conversion_fcn_params must have '
'the same number of items as of '
'thrusters')
#TODO !!!!!!!!! Check if the topic name has to be corrected somewhere !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
for i in range(self.n_thrusters):
topic = self.config['thruster_topic_prefix'].value + 'id_' + str(i) + \
self.config['thruster_topic_suffix'].value
if list not in [type(params), type(conv_fcn)]:
#Unpack parameters to values
deduced_params = {
key: val.value
for key, val in params.items()
}
thruster = Thruster.create_thruster(
self, conv_fcn, i, topic, None, None, **deduced_params)
else:
#Unpack parameters to values
deduced_params = {
key: val.value
for key, val in params[i].items()
}
thruster = Thruster.create_thruster(
self, conv_fcn[i], i, topic, None, None,
**deduced_params)
if thruster is None:
RuntimeError('Invalid thruster conversion '
'function=%s' %
self.config['conversion_fcn'].value)
self.thrusters.append(thruster)
self.get_logger().info('Thruster allocation matrix provided!')
self.get_logger().info('TAM=')
self.get_logger().info(str(self.configuration_matrix))
self.thrust = numpy.zeros(self.n_thrusters)
if not self.update_tam():
raise RuntimeError('No thrusters found')
# (pseudo) inverse: force/torque to thruster inputs
self.inverse_configuration_matrix = None
if self.configuration_matrix is not None:
self.inverse_configuration_matrix = numpy.linalg.pinv(
self.configuration_matrix)
# If an output directory was provided, store matrix for further use
if self.output_dir is not None:
with open(join(self.output_dir, 'TAM.yaml'), 'w') as yaml_file:
yaml_file.write(
yaml.safe_dump(
dict(tam=self.configuration_matrix.tolist())))
# else:
# self.get_logger().info('Invalid output directory for the TAM matrix, dir=' + str(self.output_dir))
self._ready = True
self.init_future.set_result(True)
self.get_logger().info('ThrusterManager: ready')
def rot_matrix(self):
"""`numpy.array`: Rotation matrix"""
return quaternion_matrix(self._rot)[0:3, 0:3]
def __init__(self):
"""Class constructor."""
# This flag will be set to true once the thruster allocation matrix is
# available
self._ready = False
# Acquiring the namespace of the vehicle
self.namespace = rospy.get_namespace()
if self.namespace != '':
if self.namespace[-1] != '/':
self.namespace += '/'
if self.namespace[0] != '/':
self.namespace = '/' + self.namespace
if not rospy.has_param('thruster_manager'):
raise rospy.ROSException('Thruster manager parameters not '
'initialized for uuv_name=' +
self.namespace)
# Load all parameters
self.config = rospy.get_param('thruster_manager')
robot_description_param = self.namespace + 'robot_description'
self.use_robot_descr = False
self.axes = {}
if rospy.has_param(robot_description_param):
self.use_robot_descr = True
self.parse_urdf(rospy.get_param(robot_description_param))
if self.config['update_rate'] < 0:
self.config['update_rate'] = 50
self.base_link_ned_to_enu = None
tf_buffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tf_buffer)
tf_trans_ned_to_enu = None
try:
if self.namespace != '':
target = '{}base_link'.format(self.namespace)
target = target[1::]
source = '{}base_link_ned'.format(self.namespace)
else:
target = 'base_link'
source = 'base_link_ned'
source = source[1::]
tf_trans_ned_to_enu = tf_buffer.lookup_transform(
target, source, rospy.Time(), rospy.Duration(1))
except Exception as e:
rospy.loginfo('No transform found between base_link and base_link_ned'
' for vehicle {}, message={}'.format(self.namespace, e))
self.base_link_ned_to_enu = None
if tf_trans_ned_to_enu is not None:
self.base_link_ned_to_enu = transformations.quaternion_matrix(
(tf_trans_ned_to_enu.transform.rotation.x,
tf_trans_ned_to_enu.transform.rotation.y,
tf_trans_ned_to_enu.transform.rotation.z,
tf_trans_ned_to_enu.transform.rotation.w))[0:3, 0:3]
rospy.loginfo('base_link transform NED to ENU=\n' + str(self.base_link_ned_to_enu))
rospy.loginfo(
'ThrusterManager::update_rate=' + str(self.config['update_rate']))
# Set the tf_prefix parameter
rospy.set_param('thruster_manager/tf_prefix', self.namespace)
# Retrieve the output file path to store the TAM
# matrix for future use
self.output_dir = None
if rospy.has_param('~output_dir'):
self.output_dir = rospy.get_param('~output_dir')
if not isdir(self.output_dir):
raise rospy.ROSException(
'Invalid output directory, output_dir=' + self.output_dir)
rospy.loginfo('output_dir=' + self.output_dir)
# Number of thrusters
self.n_thrusters = 0
# Thruster objects used to calculate the right angular velocity command
self.thrusters = list()
# Thrust forces vector
self.thrust = None
# Thruster allocation matrix: transform thruster inputs to force/torque
self.configuration_matrix = None
if rospy.has_param('~tam'):
tam = rospy.get_param('~tam')
self.configuration_matrix = numpy.array(tam)
# Set number of thrusters from the number of columns
self.n_thrusters = self.configuration_matrix.shape[1]
# Create publishing topics to each thruster
params = self.config['conversion_fcn_params']
conv_fcn = self.config['conversion_fcn']
if type(params) == list and type(conv_fcn) == list:
if len(params) != self.n_thrusters or len(conv_fcn) != self.n_thrusters:
raise rospy.ROSException('Lists conversion_fcn and '
'conversion_fcn_params must have '
'the same number of items as of '
'thrusters')
for i in range(self.n_thrusters):
topic = self.config['thruster_topic_prefix'] + str(i) + \
self.config['thruster_topic_suffix']
if list not in [type(params), type(conv_fcn)]:
thruster = Thruster.create_thruster(
conv_fcn, i, topic, None, None,
**params)
else:
thruster = Thruster.create_thruster(
conv_fcn[i], i, topic, None, None,
**params[i])
if thruster is None:
rospy.ROSException('Invalid thruster conversion '
'function=%s'
% self.config['conversion_fcn'])
self.thrusters.append(thruster)
rospy.loginfo('Thruster allocation matrix provided!')
rospy.loginfo('TAM=')
rospy.loginfo(self.configuration_matrix)
self.thrust = numpy.zeros(self.n_thrusters)
if not self.update_tam():
raise rospy.ROSException('No thrusters found')
# (pseudo) inverse: force/torque to thruster inputs
self.inverse_configuration_matrix = None
if self.configuration_matrix is not None:
self.inverse_configuration_matrix = numpy.linalg.pinv(
self.configuration_matrix)
# If an output directory was provided, store matrix for further use
if self.output_dir is not None:
with open(join(self.output_dir, 'TAM.yaml'), 'w') as yaml_file:
yaml_file.write(
yaml.safe_dump(
dict(tam=self.configuration_matrix.tolist())))
else:
rospy.loginfo('Invalid output directory for the TAM matrix, dir=' + str(self.output_dir))
self.ready = True
rospy.loginfo('ThrusterManager: ready')
def __init__(self,
node: Node,
inertial_frame_id='world',
tf_trans_world_ned_to_enu: TransformStamped = None):
"""Class constructor."""
self.node = node
assert inertial_frame_id in ['world', 'world_ned']
# Reading current namespace
self._namespace = self.node.get_namespace()
self._inertial_frame_id = inertial_frame_id
self._body_frame_id = None
self._logger = get_logger()
if self._inertial_frame_id == 'world':
self._body_frame_id = 'base_link'
else:
self._body_frame_id = 'base_link_ned'
# Handle tf2 results here
if tf_trans_world_ned_to_enu is not None:
self.q_ned_to_enu = np.array([
tf_trans_world_ned_to_enu.transform.rotation.x,
tf_trans_world_ned_to_enu.transform.rotation.y,
tf_trans_world_ned_to_enu.transform.rotation.z,
tf_trans_world_ned_to_enu.transform.rotation.w
])
else:
self._logger.info(
'No world ned to enu was provided. Setting to default')
self.q_ned_to_enu = quaternion_from_euler(2 * np.pi, 0, np.pi)
# try:
# import tf2_ros
# tf_buffer = tf2_ros.Buffer()
# listener = tf2_ros.TransformListener(tf_buffer, self.node)
# tf_trans_ned_to_enu = tf_buffer.lookup_transform(
# 'world', 'world_ned', rclpy.time.Time(),
# rclpy.time.Duration(seconds=10))
# self.q_ned_to_enu = np.array(
# [tf_trans_ned_to_enu.transform.rotation.x,
# tf_trans_ned_to_enu.transform.rotation.y,
# tf_trans_ned_to_enu.transform.rotation.z,
# tf_trans_ned_to_enu.transform.rotation.w])
# except Exception as ex:
# self._logger.warning(
# 'Error while requesting ENU to NED transform'
# ', message={}'.format(ex))
# self.q_ned_to_enu = quaternion_from_euler(2 * np.pi, 0, np.pi)
self.transform_ned_to_enu = quaternion_matrix(self.q_ned_to_enu)[0:3,
0:3]
if self.transform_ned_to_enu is not None:
self._logger.info('Transform world_ned (NED) to world (ENU)=\n' +
str(self.transform_ned_to_enu))
self._mass = 0.0
if self.node.has_parameter('mass'):
self._mass = self.node.get_parameter('mass').value
if self._mass <= 0:
raise RuntimeError('Mass has to be positive')
self._inertial = dict(ixx=0, iyy=0, izz=0, ixy=0, ixz=0, iyz=0)
if self.node.has_parameter('inertial'):
#inertial = self.node.get_parameter('inertial').value
inertial = self.node.get_parameters_by_prefix('inertial')
inertial = {key: val.value for key, val in inertial.items()}
for key in self._inertial:
if key not in inertial:
raise RuntimeError('Invalid moments of inertia')
self._inertial = inertial
self._cog = [0, 0, 0]
if self.node.has_parameter('cog'):
self._cog = self.node.get_parameter('cog').value
if len(self._cog) != 3:
raise RuntimeError('Invalid center of gravity vector')
self._cob = [0, 0, 0]
# bug fix wrt the original code
if self.node.has_parameter('cob'):
self._cob = self.node.get_parameter('cob').value
if len(self._cob) != 3:
raise RuntimeError('Invalid center of buoyancy vector')
self._body_frame = 'base_link'
if self.node.has_parameter('base_link'):
self._body_frame = self.node.get_parameter(
'base_link').get_parameter_value().string_value
self._volume = 0.0
if self.node.has_parameter('volume'):
self._volume = self.node.get_parameter('volume').value
if self._volume <= 0:
raise RuntimeError('Invalid volume')
# Fluid density
self._density = 1028.0
if self.node.has_parameter('density'):
self._density = self.node.get_parameter('density').value
if self._density <= 0:
raise RuntimeError('Invalid fluid density')
# Bounding box
self._height = 0.0
self._length = 0.0
self._width = 0.0
if self.node.has_parameter('height'):
self._height = self.node.get_parameter('height').value
if self._height <= 0:
raise RuntimeError('Invalid height')
if self.node.has_parameter('length'):
self._length = self.node.get_parameter('length').value
if self._length <= 0:
raise RuntimeError('Invalid length')
if self.node.has_parameter('width'):
self._width = self.node.get_parameter('width').value
if self._width <= 0:
raise RuntimeError('Invalid width')
# Calculating the rigid-body mass matrix
self._M = np.zeros(shape=(6, 6), dtype=float)
self._M[0:3, 0:3] = self._mass * np.eye(3)
self._M[0:3, 3:6] = - self._mass * \
cross_product_operator(self._cog)
self._M[3:6, 0:3] = self._mass * \
cross_product_operator(self._cog)
self._M[3:6, 3:6] = self._calc_inertial_tensor()
# Loading the added-mass matrix
self._Ma = np.zeros((6, 6))
if self.node.has_parameter('Ma'):
# Get 1D array
Ma = self.node.get_parameter('Ma').value
# Reshape the array.
self._Ma = np.reshape(Ma, (6, 6))
if self._Ma.shape != (6, 6):
raise RuntimeError('Invalid added mass matrix')
# Sum rigid-body and added-mass matrices
self._Mtotal = np.zeros(shape=(6, 6))
self._calc_mass_matrix()
# Acceleration of gravity
self._gravity = 9.81
# Initialize the Coriolis and centripetal matrix
self._C = np.zeros((6, 6))
# Vector of restoring forces
self._g = np.zeros(6)
# Loading the linear damping coefficients
self._linear_damping = np.zeros(shape=(6, 6))
if self.node.has_parameter('linear_damping'):
self._linear_damping = np.array(
self.node.get_parameter('linear_damping').value)
if self._linear_damping.shape == (6, ):
self._linear_damping = np.diag(self._linear_damping)
if self._linear_damping.shape != (6, 6):
raise RuntimeError(
'Linear damping must be given as a 6x6 matrix or the diagonal coefficients'
)
# Loading the nonlinear damping coefficients
self._quad_damping = np.zeros(shape=(6, ))
if self.node.has_parameter('quad_damping'):
self._quad_damping = np.array(
self.node.get_parameter('quad_damping').value)
if self._quad_damping.shape != (6, ):
raise RuntimeError(
'Quadratic damping must be given defined with 6 coefficients'
)
# Loading the linear damping coefficients proportional to the forward speed
self._linear_damping_forward_speed = np.zeros(shape=(6, 6))
if self.node.has_parameter('linear_damping_forward_speed'):
self._linear_damping_forward_speed = np.array(
self.node.get_parameter('linear_damping_forward_speed').value)
if self._linear_damping_forward_speed.shape == (6, ):
self._linear_damping_forward_speed = np.diag(
self._linear_damping_forward_speed)
if self._linear_damping_forward_speed.shape != (6, 6):
raise RuntimeError('Linear damping proportional to the '
'forward speed must be given as a 6x6 '
'matrix or the diagonal coefficients')
# Initialize damping matrix
self._D = np.zeros((6, 6))
# Vehicle states
self._pose = dict(pos=np.zeros(3), rot=quaternion_from_euler(0, 0, 0))
# Velocity in the body frame
self._vel = np.zeros(6)
# Acceleration in the body frame
self._acc = np.zeros(6)
# Generalized forces
self._gen_forces = np.zeros(6)
def __init__(self):
# Acquiring the namespace of the vehicle
self.namespace = rospy.get_namespace().replace('/', '')
rospy.loginfo('Initialize control allocator for vehicle <%s>' %
self.namespace)
self.tf_buffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tf_buffer)
tf_trans_ned_to_enu = None
try:
if self.namespace != '':
target = '{}/base_link'.format(self.namespace)
source = '{}/base_link_ned'.format(self.namespace)
else:
target = 'base_link'
source = 'base_link_ned'
rospy.loginfo('Lookup transfrom from %s to %s' % (source, target))
tf_trans_ned_to_enu = self.tf_buffer.lookup_transform(
target, source, rospy.Time(), rospy.Duration(1))
except Exception as e:
rospy.logwarn(
'No transform found between base_link and base_link_ned'
' for vehicle {}, message={}'.format(self.namespace, e))
self.base_link_ned_to_enu = None
if tf_trans_ned_to_enu is not None:
self.base_link_ned_to_enu = quaternion_matrix(
(tf_trans_ned_to_enu.transform.rotation.x,
tf_trans_ned_to_enu.transform.rotation.y,
tf_trans_ned_to_enu.transform.rotation.z,
tf_trans_ned_to_enu.transform.rotation.w))[0:3, 0:3]
rospy.logwarn('base_link transform NED to ENU=\n{}'.format(
self.base_link_ned_to_enu))
self.base_link = rospy.get_param('~base_link', 'base_link')
# Check if the thruster configuration is available
if not rospy.has_param('~thruster_config'):
raise rospy.ROSException('Thruster configuration not available')
# Retrieve the thruster configuration parameters
self.thruster_config = rospy.get_param('~thruster_config')
# Check if all necessary thruster model parameter are available
thruster_params = [
'conversion_fcn_params', 'conversion_fcn', 'topic_prefix',
'topic_suffix', 'frame_base', 'max_thrust'
]
for p in thruster_params:
if p not in self.thruster_config:
raise rospy.ROSException(
'Parameter <%s> for thruster conversion function is missing'
% p)
# Setting up the thruster topic name
self.thruster_topic = '/%s/%s/%d/%s' % (
self.namespace, self.thruster_config['topic_prefix'], 0,
self.thruster_config['topic_suffix'])
self.thruster = None
# Check if the fin configuration is available
if not rospy.has_param('~fin_config'):
raise rospy.ROSException('Fin configuration is not available')
# Retrieve the fin configuration is available
self.fin_config = rospy.get_param('~fin_config')
# Check if all necessary fin parameters are available
fin_params = [
'fluid_density', 'lift_coefficient', 'fin_area', 'topic_prefix',
'topic_suffix', 'frame_base'
]
for p in fin_params:
if p not in self.fin_config:
raise rospy.ROSException(
'Parameter <%s> for fin configuration is missing' % p)
self.fin_lower_limit = -np.pi / 2
if 'lower_limit' in self.fin_config:
self.fin_lower_limit = self.fin_config['lower_limit']
self.fin_upper_limit = np.pi / 2
if 'upper_limit' in self.fin_config:
self.fin_upper_limit = self.fin_config['upper_limit']
if self.fin_config['lower_limit'] >= self.fin_config['upper_limit']:
raise rospy.ROSException('Fin angle limits are invalid')
self.fins = dict()
self.n_fins = 0
if not self.find_actuators():
raise rospy.ROSException('No thruster and/or fins found')
