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, *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!')