def __init__(self):
self._tau = np.zeros(6)
self._tau1 = np.zeros(6)
DPPIDControllerBase.__init__(self, False)
self._is_init = True
self._pid_control = np.zeros(6)
self._last_vel = np.zeros(6)
self._last_t = None
self._logger.info(self._LABEL + ' ready')
def __init__(self):
DPPIDControllerBase.__init__(self, True)
self._logger.info('Initializing: ' + self._LABEL)
# Control forces and torques
self._tau = np.zeros(6)
# PID control vector
self._pid_control = np.zeros(6)
self._is_init = True
self._logger.info(self._LABEL + ' ready')
def __init__(self):
DPPIDControllerBase.__init__(self, True)
self._logger.info('Initializing: ' + self._LABEL)
# Control forces and torques
self._tau = np.zeros(6)
# PID control vector
self._pid_control = np.zeros(6)
self._is_init = True
self._logger.info(self._LABEL + ' ready')
def __init__(self, node_name, **kwargs):
DPPIDControllerBase.__init__(self, node_name, True, **kwargs)
self._logger.info('Initializing: ' + self._LABEL)
# Control forces and torques
self._tau = np.zeros(6)
# PID control vector
self._pid_control = np.zeros(6)
self._is_init = True
self._last_vel = np.zeros(6)
self._last_t = None
self._logger.info(self._LABEL + ' ready')
def __init__(self):
DPPIDControllerBase.__init__(self, True)
self._logger.info('Initializing: Model-based feedback linearization'
' controller')
# Control forces and torques
self._tau = np.zeros(6)
# PID control vector
self._pid_control = np.zeros(6)
self._is_init = True
self._logger.info(
'Model-based feedback linearization controller ready')
def __init__(self):
DPPIDControllerBase.__init__(self, True)
# Feedback acceleration gain
self._Hm = np.eye(6)
if rospy.has_param('Hm'):
hm = rospy.get_param('Hm')
if len(hm) == 6:
self._Hm = self._vehicle_model.Mtotal + np.diag(hm)
else:
raise rospy.ROSException(
'Invalid feedback acceleration gain coefficients')
# Acceleration feedback term
self._accel_ff = np.zeros(6)
# PID control vector
self._pid_control = np.zeros(6)
def __init__(self):
DPPIDControllerBase.__init__(self, True)
self._logger.info('Initializing: ' + self._LABEL)
# Feedback acceleration gain
self._Hm = np.eye(6)
if rospy.has_param('Hm'):
hm = rospy.get_param('Hm')
if len(hm) == 6:
self._Hm = self._vehicle_model.Mtotal + np.diag(hm)
else:
raise rospy.ROSException('Invalid feedback acceleration gain coefficients')
self._tau = np.zeros(6)
# Acceleration feedback term
self._accel_ff = np.zeros(6)
# PID control vector
self._pid_control = np.zeros(6)
self._is_init = True
self._logger.info(self._LABEL + ' ready')
def __init__(self, node_name, **kwargs):
DPPIDControllerBase.__init__(self, node_name, True, **kwargs)
self._logger.info('Initializing: ' + self._LABEL)
# Feedback acceleration gain
self._Hm = np.eye(6)
if self.has_parameter('Hm'):
hm = self.get_parameter('Hm').value
if len(hm) == 6:
self._Hm = self._vehicle_model.Mtotal + np.diag(hm)
else:
raise RuntimeError('Invalid feedback acceleration gain coefficients')
self._tau = np.zeros(6)
# Acceleration feedback term
self._accel_ff = np.zeros(6)
# PID control vector
self._pid_control = np.zeros(6)
self._is_init = True
self._logger.info(self._LABEL + ' ready')
def __init__(self):
self._tau = np.zeros(6)
DPPIDControllerBase.__init__(self, False)
self._is_init = True
def __init__(self, node_name, **kwargs):
self._tau = np.zeros(6)
DPPIDControllerBase.__init__(self, node_name, False, **kwargs)
self._is_init = True
def __init__(self):
DPPIDControllerBase.__init__(self, True)
self._logger.info('Initializing: ' + self._LABEL)
# Control forces and torques
self._tau = np.zeros(6)
# PID control vector
self._is_init = True
self._logger.info(self._LABEL + ' ready')
self._first_pass = True
self._t_init = 0.0
self._s_linear_b_init = np.array([0, 0, 0])
self._s_angular_b_init = np.array([0, 0, 0])
# 'K' gains (help in the initial transient)
self._K = np.zeros(6)
# Derivative gains
self._Kd = np.zeros(6)
# Robustness gains
self._Ki = np.zeros(6)
# Overall proportional gains
self._slope = np.zeros(6)
if rospy.has_param('~K'):
coefs = rospy.get_param('~K')
if len(coefs) == 6:
self._K = np.array(coefs)
else:
raise rospy.ROSException('K coefficients: 6 coefficients '
'needed')
self._logger.info('K=' + str(self._K))
if rospy.has_param('~Kd'):
coefs = rospy.get_param('~Kd')
if len(coefs) == 6:
self._Kd = np.array(coefs)
else:
raise rospy.ROSException('Kd coefficients: 6 coefficients '
'needed')
self._logger.info('Kd=' + str(self._Kd))
if rospy.has_param('~Ki'):
coefs = rospy.get_param('~Ki')
if len(coefs) == 6:
self._Ki = np.array(coefs)
else:
raise rospy.ROSException('Ki coeffcients: 6 coefficients '
'needed')
self._logger.info('Ki=' + str(self._Ki))
if rospy.has_param('~slope'):
coefs = rospy.get_param('~slope')
if len(coefs) == 6:
self._slope = np.array(coefs)
else:
raise rospy.ROSException('Slope coefficients: 6 coefficients '
'needed')
self._logger.info('slope=' + str(self._slope))
self._sat_epsilon = 0.8
if rospy.has_param('~sat_epsilon'):
self._sat_epsilon = max(0.0, rospy.get_param('~sat_epsilon'))
self._logger.info('Saturation limits=' + str(self._sat_epsilon))
self._summ_sign_sn_linear_b = np.array([0, 0, 0])
self._summ_sign_sn_angular_b = np.array([0, 0, 0])
self._prev_sign_sn_linear_b = np.array([0, 0, 0])
self._prev_sign_sn_angular_b = np.array([0, 0, 0])
self._tauForce = np.zeros(3)
self._services['set_sm_controller_params'] = rospy.Service(
'set_sm_controller_params', SetSMControllerParams,
self.set_sm_controller_params_callback)
self._services['get_sm_controller_params'] = rospy.Service(
'get_sm_controller_params', GetSMControllerParams,
self.get_sm_controller_params_callback)
self._logger.info(self._LABEL + ' ready')
def __init__(self):
self._tau = np.zeros(6)
DPPIDControllerBase.__init__(self, False)
self._is_init = True