def add_meshes(self, models): for model in models: if model in self._model_paths: print('Model %s already exists' % model) continue new_model = dict() new_model['position'] = [0, 0, 0] new_model['orientation'] = [0, 0, 0, 1] new_model['scale'] = [1, 1, 1] if 'pose' in models[model]: if 'position' in models[model]['pose']: if len(models[model]['pose']['position']) == 3: new_model['position'] = models[model]['pose']['position'] if 'orientation' in models[model]['pose']: if len(models[model]['pose']['orientation']) == 3: new_model['orientation'] = quaternion_from_euler(*models[model]['pose']['orientation']) if 'scale' in models[model]: if len(models[model]['scale']) == 3: new_model['scale'] = models[model]['scale'] if 'mesh' in models[model]: new_model['mesh'] = models[model]['mesh'] if 'model' in models[model]: model_name = models[model]['model'] prop = self._get_model_state(model_name, '') if prop.success: new_model['position'] = [prop.pose.position.x, prop.pose.position.y, prop.pose.position.z] new_model['orientation'] = [prop.pose.orientation.x, prop.pose.orientation.y, prop.pose.orientation.z, prop.pose.orientation.w] else: print('Model %s not found in the current Gazebo scenario' % model) else: print('Information about the model %s for the mesh %s could not be ' 'retrieved' % (model, models[model]['mesh'])) elif 'plane' in models[model]: new_model['plane'] = [1, 1, 1] if 'plane' in models[model]: if len(models[model]['plane']) == 3: new_model['plane'] = models[model]['plane'] else: print('Invalid scale vector for ' + model) else: continue self._model_paths[model] = new_model print('New model being published: %s' % model) print('\t Position: ' + str(self._model_paths[model]['position'])) print('\t Orientation: ' + str(self._model_paths[model]['orientation'])) print('\t Scale: ' + str(self._model_paths[model]['scale']))
def from_dict(self, data): """Initialize the trajectory point attributes from a `dict`. > *Input arguments* * `data` (*type:* `dict`): Trajectory point as a `dict` """ self._t = data['time'] self._pos = np.array(data['pos']) if self._rot.size == 3: self._rot = quaternion_from_euler(data['rot']) else: self._rot = np.array(data['rot']) self._vel = np.array(data['vel']) self._acc = np.array(data['acc'])
def send(self, msg): """ Assuming that forward is +y lateral right is +x depth up is +z """ data = PoseStamped() data.header.stamp = self.get_clock().now().to_msg() data.pose.position = Point(x=msg[0], y=msg[1], z=msg[2]) ori = transf.quaternion_from_euler(msg[3], msg[4], msg[5], 'sxyz') data.pose.orientation = Quaternion(x=ori[0], y=ori[1], z=ori[2], w=ori[3]) self.pub.publish(data)
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 rot(self, new_rot): self._rot = quaternion_from_euler(*new_rot)
def add_meshes(self, models): for model in models: if model in self._model_paths: self.get_logger().info('Model %s already exists' % model) continue new_model = dict() new_model['position'] = [0., 0., 0.] new_model['orientation'] = [0., 0., 0., 1.] new_model['scale'] = [1., 1., 1.] # It would probably be cleaner not to consider maps of maps, but rather # to create a function to parse arg1.arg2.arg3 if 'pose' in models[model]: if 'position' in models[model]['pose']: val = models[model]['pose']['position'].value if len(val) == 3: new_model['position'] = models[model]['pose'][ 'position'].value if 'orientation' in models[model]['pose']: if len(models[model]['pose']['orientation'].value) == 3: new_model['orientation'] = quaternion_from_euler( *models[model]['pose']['orientation'].value) if 'scale' in models[model]: if len(models[model]['scale'].value) == 3: new_model['scale'] = models[model]['scale'].value if 'mesh' in models[model]: new_model['mesh'] = models[model]['mesh'].value if 'model' in models[model]: model_name = models[model]['model'].value req = GetEntityState.Request() req._name = model_name req.reference_frame = '' self.get_logger().debug('Asking to: ' + self.get_entity_state.srv_name + ' model: ' + model_name) future = self.get_entity_state.call_async(req) rclpy.spin_until_future_complete(self, future) if future.result() is not None: prop = future.result() if prop.success: new_model['position'] = [ prop.state.pose.position.x, prop.state.pose.position.y, prop.state.pose.position.z ] new_model['orientation'] = [ prop.state.pose.orientation.x, prop.state.pose.orientation.y, prop.state.pose.orientation.z, prop.state.pose.orientation.w ] else: self.get_logger().info( 'Model %s not found in the current Gazebo scenario' % model) else: raise RuntimeError( 'Exception while calling service: %s' % future.exception()) else: self.get_logger().info( 'Information about the model %s for the mesh %s could not be ' 'retrieved' % (model, models[model]['mesh'].value)) elif 'plane' in models[model]: new_model['plane'] = [1, 1, 1] if 'plane' in models[model]: if len(models[model]['plane'].value) == 3: new_model['plane'] = models[model]['plane'].value else: self.get_logger().warn('Invalid scale vector for ' + model) else: continue self._model_paths[model] = new_model self.get_logger().info( '\nNew model being published: %s' % model + '\n\t Position: ' + str(self._model_paths[model]['position']) + '\n\t Orientation: ' + str(self._model_paths[model]['orientation']) + '\n\t Scale: ' + str(self._model_paths[model]['scale']))
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)