def __init__(self, *args): # Start the super class DPControllerBase.__init__(self, *args) self._logger.info('Initializing: Underactuated PID controller') # Proportional gains self._Kp = np.zeros(shape=(4, 4)) # Derivative gains self._Kd = np.zeros(shape=(4, 4)) # Integral gains self._Ki = np.zeros(shape=(4, 4)) # Integrator component self._int = np.zeros(4) # Error for the vehicle pose self._error_pose = np.zeros(4) if rospy.has_param('~Kp'): Kp_diag = rospy.get_param('~Kp') if len(Kp_diag) == 4: self._Kp = np.diag(Kp_diag) else: raise rospy.ROSException('Kp matrix error: 4 coefficients ' 'needed') self._logger.info('Kp=' + str([self._Kp[i, i] for i in range(4)])) if rospy.has_param('~Kd'): Kd_diag = rospy.get_param('~Kd') if len(Kd_diag) == 4: self._Kd = np.diag(Kd_diag) else: raise rospy.ROSException('Kd matrix error: 4 coefficients ' 'needed') self._logger.info('Kd=' + str([self._Kd[i, i] for i in range(4)])) if rospy.has_param('~Ki'): Ki_diag = rospy.get_param('~Ki') if len(Ki_diag) == 4: self._Ki = np.diag(Ki_diag) else: raise rospy.ROSException('Ki matrix error: 4 coefficients ' 'needed') self._logger.info('Ki=' + str([self._Ki[i, i] for i in range(4)])) self._services['set_pid_params'] = rospy.Service( 'set_pid_params', SetPIDParams, self.set_pid_params_callback) self._services['get_pid_params'] = rospy.Service( 'get_pid_params', GetPIDParams, self.get_pid_params_callback) self._is_init = True self._logger.info('Underactuated PID controller ready!')
def __init__(self, node_name, *args, **kwargs): # Start the super class DPControllerBase.__init__(self, node_name, *args, **kwargs) self._logger.info('Initializing: Underactuated PID controller') # Proportional gains self._Kp = np.zeros(shape=(4, 4)) # Derivative gains self._Kd = np.zeros(shape=(4, 4)) # Integral gains self._Ki = np.zeros(shape=(4, 4)) # Integrator component self._int = np.zeros(4) # Error for the vehicle pose self._error_pose = np.zeros(4) if self.has_parameter('Kp'): Kp_diag = self.get_parameter('Kp').value if len(Kp_diag) == 4: self._Kp = np.diag(Kp_diag) else: raise RuntimeError('Kp matrix error: 4 coefficients ' 'needed') self._logger.info('Kp=' + str([self._Kp[i, i] for i in range(4)])) if self.has_parameter('Kd'): Kd_diag = self.get_parameter('Kd').value if len(Kd_diag) == 4: self._Kd = np.diag(Kd_diag) else: raise RuntimeError('Kd matrix error: 4 coefficients ' 'needed') self._logger.info('Kd=' + str([self._Kd[i, i] for i in range(4)])) if self.has_parameter('Ki'): Ki_diag = self.get_parameter('Ki').value if len(Ki_diag) == 4: self._Ki = np.diag(Ki_diag) else: raise RuntimeError('Ki matrix error: 4 coefficients ' 'needed') self._logger.info('Ki=' + str([self._Ki[i, i] for i in range(4)])) srv_name = 'set_pid_params' self._services[srv_name] = self.create_service( SetPIDParams, srv_name, self.set_pid_params_callback) srv_name = 'get_pid_params' self._services[srv_name] = self.create_service( GetPIDParams, ssrv_name, self.get_pid_params_callback) self._is_init = True self._logger.info('Underactuated PID controller ready!')
def __init__(self, *args): # Start the super class DPControllerBase.__init__(self, *args) # Proportional gains self._Kp = np.zeros(shape=(6, 6)) # Derivative gains self._Kd = np.zeros(shape=(6, 6)) # Integral gains self._Ki = np.zeros(shape=(6, 6)) # Integrator component self._int = np.zeros(6) # Error for the vehicle pose self._error_pose = np.zeros(6) if rospy.has_param('~Kp'): Kp_diag = rospy.get_param('~Kp') if len(Kp_diag) == 6: self._Kp = np.diag(Kp_diag) else: raise rospy.ROSException('Kp matrix error: 6 coefficients ' 'needed') print "Kp=", [self._Kp[i, i] for i in range(6)] if rospy.has_param('~Kd'): Kd_diag = rospy.get_param('~Kd') if len(Kd_diag) == 6: self._Kd = np.diag(Kd_diag) else: raise rospy.ROSException('Kd matrix error: 6 coefficients ' 'needed') print "Kd=", [self._Kd[i, i] for i in range(6)] if rospy.has_param('~Ki'): Ki_diag = rospy.get_param('~Ki') if len(Ki_diag) == 6: self._Ki = np.diag(Ki_diag) else: raise rospy.ROSException('Ki matrix error: 6 coefficients ' 'needed') print "Ki=", [self._Ki[i, i] for i in range(6)] self._services['set_pid_params'] = rospy.Service( 'set_pid_params', SetPIDParams, self.set_pid_params_callback) self._services['get_pid_params'] = rospy.Service( 'get_pid_params', GetPIDParams, self.get_pid_params_callback)
def __init__(self, *args): # Start the super class DPControllerBase.__init__(self, *args) self._logger.info('Initializing: AUV Geometric Tracking Controller') # Thrust gain self._thrust_gain = 0.0 if rospy.has_param('~thrust_gain'): self._thrust_gain = rospy.get_param('~thrust_gain') if self._thrust_gain <= 0: raise rospy.ROSException('Thrust gain must be greater than zero') self._logger.info('Thrust gain=' + str(self._thrust_gain)) # Maximum absolute fin angle displacement should be given in radians self._max_fin_angle = 0.0 if rospy.has_param('~max_fin_angle'): self._max_fin_angle = rospy.get_param('~max_fin_angle') if self._max_fin_angle <= 0.0: raise rospy.ROSException( 'Max. fin displacement angle must be greater than zero') # Proportional gain for the roll angle self._roll_gain = 0.0 if rospy.has_param('~roll_gain'): self._roll_gain = rospy.get_param('~roll_gain') if self._roll_gain <= 0: raise rospy.ROSException('Roll gain must be greater than zero') self._logger.info('Roll gain=' + str(self._roll_gain)) # Proportional gain for the pitch angle self._pitch_gain = 0.0 if rospy.has_param('~pitch_gain'): self._pitch_gain = rospy.get_param('~pitch_gain') if self._pitch_gain <= 0: raise rospy.ROSException('Pitch gain must be greater than zero') self._logger.info('Pitch gain=' + str(self._pitch_gain)) # Proportional gain for the yaw angle self._yaw_gain = 0.0 if rospy.has_param('~yaw_gain'): self._yaw_gain = rospy.get_param('~yaw_gain') if self._yaw_gain <= 0: raise rospy.ROSException('Yaw gain must be greater than zero') self._logger.info('Yaw gain=' + str(self._yaw_gain)) # Number of fins self._n_fins = 4 if rospy.has_param('~n_fins'): self._n_fins = rospy.get_param('~n_fins') if self._n_fins <= 0: raise rospy.ROSException( 'Number of fins must be greater than zero') self._logger.info('Number of fins=' + str(self._n_fins)) # Contribution of each fin to the orientation angles (roll, pitch and # yaw) self._fin_contribution_roll = [0, 0, 0, 0] self._fin_contribution_pitch = [0, 0, 0, 0] self._fin_contribution_yaw = [0, 0, 0, 0] if rospy.has_param('~fin_contribution_roll'): self._fin_contribution_roll = np.array( rospy.get_param('~fin_contribution_roll')) if self._fin_contribution_roll.size != self._n_fins: raise rospy.ROSException( 'Number of elements in the roll contribution vector must be' ' equal to the number of fins') if np.max(self._fin_contribution_roll) > 1 or \ np.min(self._fin_contribution_roll) < -1: raise rospy.ROSException('Values in the roll contribution vector' ' must be -1 <= i <= 1') if rospy.has_param('~fin_contribution_pitch'): self._fin_contribution_pitch = np.array( rospy.get_param('~fin_contribution_pitch')) if self._fin_contribution_pitch.size != self._n_fins: raise rospy.ROSException( 'Number of elements in the pitch contribution vector must be' ' equal to the number of fins') if np.max(self._fin_contribution_pitch) > 1 or \ np.min(self._fin_contribution_pitch) < -1: raise rospy.ROSException('Values in the pitch contribution vector' ' must be -1 <= i <= 1') if rospy.has_param('~fin_contribution_yaw'): self._fin_contribution_yaw = np.array( rospy.get_param('~fin_contribution_yaw')) if self._fin_contribution_yaw.size != self._n_fins: raise rospy.ROSException( 'Number of elements in the yaw contribution vector must be' ' equal to the number of fins') if np.max(self._fin_contribution_yaw) > 1 or \ np.min(self._fin_contribution_yaw) < -1: raise rospy.ROSException('Values in the yaw contribution vector' ' must be -1 <= i <= 1') # Generating vehicle orientation to fins conversion matrix self._rpy_to_fins = np.vstack( (self._fin_contribution_roll, self._fin_contribution_pitch, self._fin_contribution_yaw)).T # Prefix to the fin command topic relative to the robot model self._fin_topic_prefix = 'fins/' if rospy.has_param('~fin_topic_prefix'): self._fin_topic_prefix = rospy.get_param('~fin_topic_prefix') # Fin command topic suffix self._fin_topic_suffix = '/input' if rospy.has_param('~fin_topic_suffix'): self._fin_topic_suffix = rospy.get_param('~fin_topic_suffix') # Initialize fin command publishers self._fin_pubs = list() for i in range(self._n_fins): topic = self._fin_topic_prefix + str(i) + self._fin_topic_suffix self._fin_pubs.append( rospy.Publisher(topic, FloatStamped, queue_size=10)) self._logger.info('Publishing command to #%i fin=%s' % (i, topic)) # Suffix for the thruster topic relative to the robot model self._thruster_topic = 'thrusters/0/input' if rospy.has_param('~thruster_topic'): self._thruster_topic = rospy.get_param('~thruster_topic') # Get thruster parameters if not rospy.has_param('~thruster_model'): raise rospy.ROSException('Thruster parameters were not provided') self._thruster_params = rospy.get_param('~thruster_model') if 'max_thrust' not in self._thruster_params: raise rospy.ROSException('No limit to thruster output was given') self._thruster_model = Thruster.create_thruster( self._thruster_params['name'], 0, self._thruster_topic, None, None, **self._thruster_params['params']) self._logger.info('Publishing to thruster=%s' % self._thruster_topic) self._is_init = True self._logger.info('AUV Geometric Tracking Controller ready!')
def __init__(self, *args): # Start the super class DPControllerBase.__init__(self, *args) self._logger.info('Initializing: AUV Geometric Tracking Controller') # Thrust gain self._thrust_gain = 0.0 if rospy.has_param('~thrust_gain'): self._thrust_gain = rospy.get_param('~thrust_gain') self._min_thrust = 0.0 if rospy.has_param('~min_thrust'): self._min_thrust = rospy.get_param('~min_thrust') self._min_thrust = max(0.0, self._min_thrust) if self._thrust_gain <= 0: raise rospy.ROSException('Thrust gain must be greater than zero') self._logger.info('Thrust gain=' + str(self._thrust_gain)) # Maximum absolute fin angle displacement should be given in radians self._max_fin_angle = 0.0 if rospy.has_param('~max_fin_angle'): self._max_fin_angle = rospy.get_param('~max_fin_angle') if self._max_fin_angle <= 0.0: raise rospy.ROSException('Max. fin displacement angle must be greater than zero') # Proportional gain for the roll angle self._roll_gain = 0.0 if rospy.has_param('~roll_gain'): self._roll_gain = rospy.get_param('~roll_gain') if self._roll_gain <= 0: raise rospy.ROSException('Roll gain must be greater than zero') self._logger.info('Roll gain=' + str(self._roll_gain)) # Proportional gain for the pitch angle self._pitch_gain = 0.0 if rospy.has_param('~pitch_gain'): self._pitch_gain = rospy.get_param('~pitch_gain') if self._pitch_gain <= 0: raise rospy.ROSException('Pitch gain must be greater than zero') self._logger.info('Pitch gain=' + str(self._pitch_gain)) if not rospy.has_param('~pitch_abs_saturation'): raise rospy.ROSException('Pitch absolute saturation missing') self._pitch_abs_saturation = rospy.get_param('~pitch_abs_saturation') if self._pitch_abs_saturation <= 0: raise rospy.ROSException('Absolute saturation for pitch must be' ' greater than zero') # Proportional gain for the yaw angle self._yaw_gain = 0.0 if rospy.has_param('~yaw_gain'): self._yaw_gain = rospy.get_param('~yaw_gain') if self._yaw_gain <= 0: raise rospy.ROSException('Yaw gain must be greater than zero') self._logger.info('Yaw gain=' + str(self._yaw_gain)) if not rospy.has_param('~yaw_abs_saturation'): raise rospy.ROSException('Yaw absolute saturation missing') self._yaw_abs_saturation = rospy.get_param('~yaw_abs_saturation') if self._yaw_abs_saturation <= 0: raise rospy.ROSException('Absolute saturation for yaw must be' ' greater than zero') # Number of fins self._n_fins = 4 if rospy.has_param('~n_fins'): self._n_fins = rospy.get_param('~n_fins') if self._n_fins <= 0: raise rospy.ROSException('Number of fins must be greater than zero') self._logger.info('Number of fins=' + str(self._n_fins)) # Contribution of each fin to the orientation angles (roll, pitch and # yaw) self._fin_contribution_roll = [0, 0, 0, 0] self._fin_contribution_pitch = [0, 0, 0, 0] self._fin_contribution_yaw = [0, 0, 0, 0] if rospy.has_param('~fin_contribution_roll'): self._fin_contribution_roll = np.array( rospy.get_param('~fin_contribution_roll')) if self._fin_contribution_roll.size != self._n_fins: raise rospy.ROSException( 'Number of elements in the roll contribution vector must be' ' equal to the number of fins') if np.max(self._fin_contribution_roll) > 1 or \ np.min(self._fin_contribution_roll) < -1: raise rospy.ROSException('Values in the roll contribution vector' ' must be -1 <= i <= 1') if rospy.has_param('~fin_contribution_pitch'): self._fin_contribution_pitch = np.array( rospy.get_param('~fin_contribution_pitch')) if self._fin_contribution_pitch.size != self._n_fins: raise rospy.ROSException( 'Number of elements in the pitch contribution vector must be' ' equal to the number of fins') if np.max(self._fin_contribution_pitch) > 1 or \ np.min(self._fin_contribution_pitch) < -1: raise rospy.ROSException('Values in the pitch contribution vector' ' must be -1 <= i <= 1') if rospy.has_param('~fin_contribution_yaw'): self._fin_contribution_yaw = np.array( rospy.get_param('~fin_contribution_yaw')) if self._fin_contribution_yaw.size != self._n_fins: raise rospy.ROSException( 'Number of elements in the yaw contribution vector must be' ' equal to the number of fins') if np.max(self._fin_contribution_yaw) > 1 or \ np.min(self._fin_contribution_yaw) < -1: raise rospy.ROSException('Values in the yaw contribution vector' ' must be -1 <= i <= 1') # Generating vehicle orientation to fins conversion matrix self._rpy_to_fins = np.vstack((self._fin_contribution_roll, self._fin_contribution_pitch, self._fin_contribution_yaw)).T # Prefix to the fin command topic relative to the robot model self._fin_topic_prefix = 'fins/' if rospy.has_param('~fin_topic_prefix'): self._fin_topic_prefix = rospy.get_param('~fin_topic_prefix') # Fin command topic suffix self._fin_topic_suffix = '/input' if rospy.has_param('~fin_topic_suffix'): self._fin_topic_suffix = rospy.get_param('~fin_topic_suffix') # Initialize fin command publishers self._fin_pubs = list() for i in range(self._n_fins): topic = self._fin_topic_prefix + str(i) + self._fin_topic_suffix self._fin_pubs.append(rospy.Publisher(topic, FloatStamped, queue_size=10)) self._logger.info('Publishing command to #%i fin=%s' % (i, topic)) # Suffix for the thruster topic relative to the robot model self._thruster_topic = 'thrusters/0/input' if rospy.has_param('~thruster_topic'): self._thruster_topic = rospy.get_param('~thruster_topic') # Get thruster parameters if not rospy.has_param('~thruster_model'): raise rospy.ROSException('Thruster parameters were not provided') self._thruster_params = rospy.get_param('~thruster_model') if 'max_thrust' not in self._thruster_params: raise rospy.ROSException('No limit to thruster output was given') self._thruster_model = Thruster.create_thruster( self._thruster_params['name'], 0, self._thruster_topic, None, None, **self._thruster_params['params']) self._logger.info('Publishing to thruster=%s' % self._thruster_topic) self._is_init = True self._logger.info('AUV Geometric Tracking Controller ready!')