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')