def __init__(self):
# Start the super class
DPControllerBase.__init__(self, is_model_based=True)
self._logger.info('Initializing: ' + self._LABEL)
# Proportional gains
self._Kp = np.zeros(shape=(6, 6))
# Derivative gains
self._Kd = np.zeros(shape=(6, 6))
# Error for the vehicle pose
self._error_pose = np.zeros(6)
self._tau = 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')
self._logger.info('Kp=' + str([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')
self._logger.info('Kd=' + str([self._Kd[i, i] for i in range(6)]))
self._is_init = True
self._logger.info(self._LABEL + ' ready')
def __init__(self):
# Start the super class
DPControllerBase.__init__(self, is_model_based=True)
self._logger.info('Initializing: ' + self._LABEL)
# Proportional gains
self._Kp = np.zeros(shape=(6, 6))
# Derivative gains
self._Kd = np.zeros(shape=(6, 6))
# Error for the vehicle pose
self._error_pose = np.zeros(6)
self._tau = 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')
self._logger.info('Kp=' + str([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')
self._logger.info('Kd=' + str([self._Kd[i, i] for i in range(6)]))
self._is_init = True
self._logger.info(self._LABEL + ' ready')
def __init__(self, node_name, **kwargs):
DPControllerBase.__init__(self, node_name, True, **kwargs)
self._tau = np.zeros(6)
self._logger.info('Initializing: ' + self._LABEL)
self._Kd = np.zeros(shape=(6, 6))
if self.has_parameter('Kd'):
coefs = self.get_parameter('Kd').value
if len(coefs) == 6:
self._Kd = np.diag(coefs)
else:
raise RuntimeError('Kd coefficients: 6 coefficients '
'needed')
self._logger.info('Kd=\n' + str(self._Kd))
# Build delta matrix
self._delta = np.zeros(shape=(6, 6))
l = [0.0]
if self.has_parameter('lambda'):
l = self.get_parameter('lambda').value
# Turn l into a list if it is not the case
if type(l) is not list:
l = [l]
if len(l) == 1:
self._delta[0:3, 0:3] = l[0] * np.eye(3)
elif len(l) == 3:
self._delta[0:3, 0:3] = np.diag(l)
else:
raise RuntimeError(
'lambda: either a scalar or a 3 element vector must be provided')
c = [0.0]
if self.has_parameter('c'):
c = self.get_parameter('c').value
# Turn c into a list if it is not the case
if type(c) is not list:
c = [c]
if len(c) == 1:
self._delta[3:6, 3:6] = c[0] * np.eye(3)
elif len(c) == 3:
self._delta[3:6, 3:6] = np.diag(c)
else:
raise RuntimeError(
'c: either a scalar or a 3 element vector must be provided')
self._logger.info('delta=\n' + str(self._delta))
self._prev_t = None
self._prev_vel_r = np.zeros(6)
self._is_init = True
self._logger.info(self._LABEL + ' ready')
def __init__(self):
DPControllerBase.__init__(self, True)
self._tau = np.zeros(6)
self._logger.info('Initializing: ' + self._LABEL)
self._Kd = np.zeros(shape=(6, 6))
if rospy.has_param('~Kd'):
coefs = rospy.get_param('~Kd')
if len(coefs) == 6:
self._Kd = np.diag(coefs)
else:
raise rospy.ROSException('Kd coefficients: 6 coefficients '
'needed')
self._logger.info('Kd=' + str(self._Kd))
# Build delta matrix
self._delta = np.zeros(shape=(6, 6))
l = rospy.get_param('~lambda', [0.0])
if len(l) == 1:
self._delta[0:3, 0:3] = l[0] * np.eye(3)
elif len(l) == 3:
self._delta[0:3, 0:3] = np.diag(l)
else:
raise rospy.ROSException(
'lambda: either a scalar or a 3 element vector must be provided'
)
c = rospy.get_param('~c', [0.0])
if len(c) == 1:
self._delta[3:6, 3:6] = c[0] * np.eye(3)
elif len(c) == 3:
self._delta[3:6, 3:6] = np.diag(c)
else:
raise rospy.ROSException(
'c: either a scalar or a 3 element vector must be provided')
self._logger.info('delta=' + str(self._delta))
self._prev_t = None
self._prev_vel_r = np.zeros(6)
self._is_init = True
self._logger.info(self._LABEL + ' ready')
def __init__(self):
DPControllerBase.__init__(self, True)
self._tau = np.zeros(6)
self._logger.info('Initializing: ' + self._LABEL)
self._Kd = np.zeros(shape=(6, 6))
if rospy.has_param('~Kd'):
coefs = rospy.get_param('~Kd')
if len(coefs) == 6:
self._Kd = np.diag(coefs)
else:
raise rospy.ROSException('Kd coefficients: 6 coefficients '
'needed')
self._logger.info('Kd=\n' + str(self._Kd))
# Build delta matrix
self._delta = np.zeros(shape=(6, 6))
l = rospy.get_param('~lambda', [0.0])
if len(l) == 1:
self._delta[0:3, 0:3] = l[0] * np.eye(3)
elif len(l) == 3:
self._delta[0:3, 0:3] = np.diag(l)
else:
raise rospy.ROSException(
'lambda: either a scalar or a 3 element vector must be provided')
c = rospy.get_param('~c', [0.0])
if len(c) == 1:
self._delta[3:6, 3:6] = c[0] * np.eye(3)
elif len(c) == 3:
self._delta[3:6, 3:6] = np.diag(c)
else:
raise rospy.ROSException(
'c: either a scalar or a 3 element vector must be provided')
self._logger.info('delta=\n' + str(self._delta))
self._prev_t = None
self._prev_vel_r = np.zeros(6)
self._is_init = True
self._logger.info(self._LABEL + ' ready')
def __init__(self, node_name, **kwargs):
DPControllerBase.__init__(self, node_name, True, **kwargs)
self._logger.info('Initializing: ' + self._LABEL)
# Lambda - Slope of the Sliding Surface
self._lambda = np.zeros(6)
# Rho Constant - Vector of positive terms for assuring sliding surface reaching condition
self._rho_constant = np.zeros(6)
# k - PD gain (P term = k * lambda , D term = k)
self._k = np.zeros(6)
# c - slope of arctan (the greater, the more similar with the sign function)
self._c = np.zeros(6)
# Adapt slope - Adaptation gain for the estimation of uncertainties
# and disturbances upper boundaries
# adapt_slope = [proportional to surface distance, prop. to square
# of pose errors, prop. to square of velocity errors]
self._adapt_slope = np.zeros(3)
# Rho_0 - rho_adapt treshold for drift prevention
self._rho_0 = np.zeros(6)
# Drift prevent - Drift prevention slope
self._drift_prevent = 0
if self.has_parameter('lambda'):
coeffs = self.get_parameter('lambda').value
coeffs = [float(c) for c in coeffs]
if len(coeffs) == 6:
self._lambda = np.array(coeffs)
else:
raise RuntimeError('lambda coefficients: 6 coefficients '
'needed')
print('lambda=', self._lambda)
if self.has_parameter('rho_constant'):
coeffs = self.get_parameter('rho_constant').value
coeffs = [float(c) for c in coeffs]
if len(coeffs) == 6:
self._rho_constant = np.array(coeffs)
else:
raise RuntimeError('rho_constant coefficients: 6 coefficients '
'needed')
print('rho_constant=', self._rho_constant)
if self.has_parameter('k'):
coeffs = self.get_parameter('k').value
coeffs = [float(c) for c in coeffs]
if len(coeffs) == 6:
self._k = np.array(coeffs)
else:
raise RuntimeError('k coefficients: 6 coefficients ' 'needed')
print('k=', self._k)
if self.has_parameter('c'):
coeffs = self.get_parameter('c').value
coeffs = [float(c) for c in coeffs]
if len(coeffs) == 6:
self._c = np.array(coeffs)
else:
raise RuntimeError('c coefficients: 6 coefficients ' 'needed')
print('c=', self._c)
if self.has_parameter('adapt_slope'):
coeffs = self.get_parameter('adapt_slope').value
coeffs = [float(c) for c in coeffs]
if len(coeffs) == 3:
self._adapt_slope = np.array(coeffs)
else:
raise RuntimeError('adapt_slope coefficients: 6 coefficients '
'needed')
print('adapt_slope=', self._adapt_slope)
if self.has_parameter('rho_0'):
coeffs = self.get_parameter('rho_0').value
coeffs = [float(c) for c in coeffs]
if len(coeffs) == 6:
self._rho_0 = np.array(coeffs)
else:
raise RuntimeError('rho_0 coefficients: 6 coefficients '
'needed')
print('rho_0=', self._rho_0)
if self.has_parameter('drift_prevent'):
scalar = self.get_parameter('drift_prevent').value
coeffs = [float(c) for c in coeffs]
if not isinstance(scalar, list):
self._drift_prevent = scalar
else:
raise RuntimeError('drift_prevent needs to be a scalar value')
print('drift_prevent=', self._drift_prevent)
# Enable(1) / disable(0) integral term in the sliding surface
if self.has_parameter('enable_integral_term'):
self._sliding_int = self.get_parameter(
'enable_integral_term').get_parameter_value().integer_value
else:
self._sliding_int = 0
# Enable(1) / disable(0) adaptive uncertainty upper boundaries for
# robust control
if self.has_parameter('adaptive_bounds'):
self._adaptive_bounds = self.get_parameter(
'adaptive_bounds').get_parameter_value().integer_value
else:
self._adaptive_bounds = 1
# Enable(1) / disable(0) constant uncertainty upper boundaries for
# robust control
if self.has_parameter('constant_bound'):
self._constant_bound = self.get_parameter(
'constant_bound').get_parameter_value().integer_value
else:
self._constant_bound = 1
# Enable(1) / disable(0) equivalent control term
if self.has_parameter('ctrl_eq'):
self._ctrl_eq = self.get_parameter(
'ctrl_eq').get_parameter_value().integer_value
else:
self._ctrl_eq = 1
# Enable(1) / disable(0) linear control term
if self.has_parameter('ctrl_lin'):
self._ctrl_lin = self.get_parameter(
'ctrl_lin').get_parameter_value().integer_value
else:
self._ctrl_lin = 1
# Enable(1) / disable(0) robust control term
if self.has_parameter('ctrl_robust'):
self._ctrl_robust = self.get_parameter(
'ctrl_robust').get_parameter_value().integer_value
else:
self._ctrl_robust = 1
# Integrator component
self._int = np.zeros(6)
# Error for the vehicle pose
self._error_pose = np.zeros(6)
# Sliding Surface
self._s_b = np.zeros(6)
# Time derivative of the rotation matrix
self._rotBtoI_dot = np.zeros(shape=(3, 3), dtype=float)
# Linear acceleration estimation
self._accel_linear_estimate_b = np.zeros(3)
# Angular acceleration estimation
self._accel_angular_estimate_b = np.zeros(3)
# Acceleration estimation
self._accel_estimate_b = np.zeros(6)
# adaptive term of uncertainties upper bound estimation
self._rho_adapt = np.zeros(6)
# Upper bound for model uncertainties and disturbances
self._rho_total = np.zeros(6)
# Equivalent control
self._f_eq = np.zeros(6)
# Linear term of controller
self._f_lin = np.zeros(6)
# Robust control
self._f_robust = np.zeros(6)
# Total control
self._tau = np.zeros(6)
srv_name = 'set_mb_sm_controller_params'
self._services[srv_name] = self.create_service(
SetMBSMControllerParams, srv_name,
self.set_mb_sm_controller_params_callback)
srv_name = 'get_mb_sm_controller_params'
self._services[srv_name] = self.create_service(
GetMBSMControllerParams, srv_name,
self.get_mb_sm_controller_params_callback)
self._is_init = True
self._logger.info(self._LABEL + ' ready')
def __init__(self):
DPControllerBase.__init__(self, is_model_based=False)
self._logger.info('Initializing: ' + self._LABEL)
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._tau = np.zeros(6)
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._is_init = True
self._logger.info(self._LABEL + ' ready')
def __init__(self, name, **kwargs):
DPControllerBase.__init__(self, name, is_model_based=False, **kwargs)
self._logger.info('Initializing: ' + self._LABEL)
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 self.has_parameter('K'):
coefs = self.get_parameter('K').value
if len(coefs) == 6:
self._K = np.array(coefs)
else:
raise RuntimeError('K coefficients: 6 coefficients ' 'needed')
self._logger.info('K=' + str(self._K))
if self.has_parameter('Kd'):
coefs = self.get_parameter('Kd').value
if len(coefs) == 6:
self._Kd = np.array(coefs)
else:
raise RuntimeError('Kd coefficients: 6 coefficients ' 'needed')
self._logger.info('Kd=' + str(self._Kd))
if self.has_parameter('Ki'):
coefs = self.get_parameter('Ki').value
if len(coefs) == 6:
self._Ki = np.array(coefs)
else:
raise RuntimeError('Ki coeffcients: 6 coefficients ' 'needed')
self._logger.info('Ki=' + str(self._Ki))
if self.has_parameter('slope'):
coefs = self.get_parameter('slope').value
if len(coefs) == 6:
self._slope = np.array(coefs)
else:
raise RuntimeError('Slope coefficients: 6 coefficients '
'needed')
self._logger.info('slope=' + str(self._slope))
self._sat_epsilon = 0.8
if self.has_parameter('sat_epsilon'):
self._sat_epsilon = max(0.0,
self.get_parameter('sat_epsilon').value)
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._tau = np.zeros(6)
srv_name = 'set_sm_controller_params'
self._services[srv_name] = self.create_service(
SetSMControllerParams,
srv_name,
self.set_sm_controller_params_callback,
callback_group=self._callback_group)
srv_name = 'get_sm_controller_params'
self._services[srv_name] = self.create_service(
GetSMControllerParams,
srv_name,
self.get_sm_controller_params_callback,
callback_group=self._callback_group)
self._is_init = True
self._logger.info(self._LABEL + ' ready')
def __init__(self):
DPControllerBase.__init__(self,
is_model_based=False,
planner_full_dof=False)
self._logger.info('Initializing: ' + self._LABEL)
self._force_torque = np.zeros(6)
self.odom_pos = np.zeros(3)
self.odom_quat = np.array([0, 0, 0, 1])
self.odom_vel_ang = np.array(3)
self.odom_vel_lin = np.array(3)
self._logger.info(self._LABEL + ' ready')
self._mass = rospy.get_param("pid/mass")
print self._mass
self._inertial = rospy.get_param("pid/inertial")
print self._inertial
self._use_cascaded_pid = rospy.get_param("use_cascaded_pid", True)
self._last_vel = np.zeros(6)
# update mass, moments of inertia
self._inertial_tensor = np.array([[
self._inertial['ixx'], self._inertial['ixy'], self._inertial['ixz']
], [
self._inertial['ixy'], self._inertial['iyy'], self._inertial['iyz']
], [
self._inertial['ixz'], self._inertial['iyz'], self._inertial['izz']
]])
self._mass_inertial_matrix = np.vstack((np.hstack(
(self._mass * np.identity(3), np.zeros(
(3, 3)))), np.hstack((np.zeros(
(3, 3)), self._inertial_tensor))))
self._velocity_pid = rospy.get_param('velocity_control')
print self._velocity_pid
self._position_pid = rospy.get_param('position_control')
print self._position_pid
self._lin_vel_pid_reg = PIDRegulator(self._velocity_pid['linear_p'], \
self._velocity_pid['linear_i'], \
self._velocity_pid['linear_d'], \
self._velocity_pid['linear_sat'])
self._ang_vel_pid_reg = PIDRegulator(self._velocity_pid['angular_p'], \
self._velocity_pid['angular_i'], \
self._velocity_pid['angular_d'], \
self._velocity_pid['angular_sat'])
self._lin_pos_pid_reg = PIDRegulator(self._position_pid['pos_p'], \
self._position_pid['pos_i'], \
self._position_pid['pos_d'], \
self._position_pid['pos_sat'])
self._ang_pos_pid_reg = PIDRegulator(self._position_pid['rot_p'], \
self._position_pid['rot_i'], \
self._position_pid['rot_d'], \
self._position_pid['rot_sat'])
self.sub_odometry = rospy.Subscriber('/anahita/pose_gt',
numpy_msg(Odometry),
self.o_callback)
self.cmd_pose_pub = rospy.Publisher('/anahita/cmd_pose',
Pose,
queue_size=10)
self._logger.info('Cascaded PID controller ready!')
self._last_t = None
self._is_init = True
def __init__(self):
DPControllerBase.__init__(self, is_model_based=False)
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')
print 'K=', 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')
print 'Kd=', 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')
print 'Ki=', 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')
print 'slope=', self._slope
self._prev_t = -1.0
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._tau = np.zeros(6)
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)
def __init__(self):
DPControllerBase.__init__(self, True)
# Lambda - Slope of the Sliding Surface
self._lambda = np.zeros(6)
# Rho Constant - Vector of positive terms for assuring sliding surface reaching condition
self._rho_constant = np.zeros(6)
# k - PD gain (P term = k * lambda , D term = k)
self._k = np.zeros(6)
# c - slope of arctan (the greater, the more similar with the sign function)
self._c = np.zeros(6)
# Adapt slope - Adaptation gain for the estimation of uncertainties
# and disturbances upper boundaries
# adapt_slope = [proportional to surface distance, prop. to square
# of pose errors, prop. to square of velocity errors]
self._adapt_slope = np.zeros(3)
# Rho_0 - rho_adapt treshold for drift prevention
self._rho_0 = np.zeros(6)
# Drift prevent - Drift prevention slope
self._drift_prevent = 0
if rospy.has_param('~lambda'):
coefs = rospy.get_param('~lambda')
if len(coefs) == 6:
self._lambda = np.array(coefs)
else:
raise rospy.ROSException('lambda coefficients: 6 coefficients '
'needed')
print 'lambda=', self._lambda
if rospy.has_param('~rho_constant'):
coefs = rospy.get_param('~rho_constant')
if len(coefs) == 6:
self._rho_constant = np.array(coefs)
else:
raise rospy.ROSException(
'rho_constant coefficients: 6 coefficients '
'needed')
print 'rho_constant=', self._rho_constant
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')
print 'k=', self._k
if rospy.has_param('~c'):
coefs = rospy.get_param('~c')
if len(coefs) == 6:
self._c = np.array(coefs)
else:
raise rospy.ROSException('c coefficients: 6 coefficients '
'needed')
print 'c=', self._c
if rospy.has_param('~adapt_slope'):
coefs = rospy.get_param('~adapt_slope')
if len(coefs) == 3:
self._adapt_slope = np.array(coefs)
else:
raise rospy.ROSException(
'adapt_slope coefficients: 6 coefficients '
'needed')
print 'adapt_slope=', self._adapt_slope
if rospy.has_param('~rho_0'):
coefs = rospy.get_param('~rho_0')
if len(coefs) == 6:
self._rho_0 = np.array(coefs)
else:
raise rospy.ROSException('rho_0 coefficients: 6 coefficients '
'needed')
print 'rho_0=', self._rho_0
if rospy.has_param('~drift_prevent'):
scalar = rospy.get_param('~drift_prevent')
if not isinstance(scalar, list):
self._drift_prevent = scalar
else:
raise rospy.ROSException(
'drift_prevent needs to be a scalar value')
print 'drift_prevent=', self._drift_prevent
# Enable(1) / disable(0) integral term in the sliding surface
if rospy.has_param('~enable_integral_term'):
self._sliding_int = rospy.get_param('~enable_integral_term')
else:
self._sliding_int = 0
# Enable(1) / disable(0) adaptive uncertainty upper boundaries for
# robust control
if rospy.has_param('~adaptive_bounds'):
self._adaptive_bounds = rospy.get_param('~adaptive_bounds')
else:
self._adaptive_bounds = 1
# Enable(1) / disable(0) constant uncertainty upper boundaries for
# robust control
if rospy.has_param('~constant_bound'):
self._constant_bound = rospy.get_param('~constant_bound')
else:
self._constant_bound = 1
# Enable(1) / disable(0) equivalent control term
if rospy.has_param('~ctrl_eq'):
self._ctrl_eq = rospy.get_param('~ctrl_eq')
else:
self._ctrl_eq = 1
# Enable(1) / disable(0) linear control term
if rospy.has_param('~ctrl_lin'):
self._ctrl_lin = rospy.get_param('~ctrl_lin')
else:
self._ctrl_lin = 1
# Enable(1) / disable(0) robust control term
if rospy.has_param('~ctrl_robust'):
self._ctrl_robust = rospy.get_param('~ctrl_robust')
else:
self._ctrl_robust = 1
# Integrator component
self._int = np.zeros(6)
# Error for the vehicle pose
self._error_pose = np.zeros(6)
# Sliding Surface
self._s_b = np.zeros(6)
# Time derivative of the rotation matrix
self._rotBtoI_dot = np.zeros(shape=(3, 3), dtype=float)
# Linear acceleration estimation
self._accel_linear_estimate_b = np.zeros(3)
# Angular acceleration estimation
self._accel_angular_estimate_b = np.zeros(3)
# Acceleration estimation
self._accel_estimate_b = np.zeros(6)
# adaptive term of uncertainties upper bound estimation
self._rho_adapt = np.zeros(6)
# Upper bound for model uncertainties and disturbances
self._rho_total = np.zeros(6)
# Equivalent control
self._f_eq = np.zeros(6)
# Linear term of controller
self._f_lin = np.zeros(6)
# Robust control
self._f_robust = np.zeros(6)
# Total control
self._tau = np.zeros(6)
self._services['set_mb_sm_controller_params'] = rospy.Service(
'set_mb_sm_controller_params', SetMBSMControllerParams,
self.set_mb_sm_controller_params_callback)
self._services['get_mb_sm_controller_params'] = rospy.Service(
'get_mb_sm_controller_params', GetMBSMControllerParams,
self.get_mb_sm_controller_params_callback)
self._is_init = True
def __init__(self):
DPControllerBase.__init__(self, True)
self._logger.info('Initializing: ' + self._LABEL)
# Lambda - Slope of the Sliding Surface
self._lambda = np.zeros(6)
# Rho Constant - Vector of positive terms for assuring sliding surface reaching condition
self._rho_constant = np.zeros(6)
# k - PD gain (P term = k * lambda , D term = k)
self._k = np.zeros(6)
# c - slope of arctan (the greater, the more similar with the sign function)
self._c = np.zeros(6)
# Adapt slope - Adaptation gain for the estimation of uncertainties
# and disturbances upper boundaries
# adapt_slope = [proportional to surface distance, prop. to square
# of pose errors, prop. to square of velocity errors]
self._adapt_slope = np.zeros(3)
# Rho_0 - rho_adapt treshold for drift prevention
self._rho_0 = np.zeros(6)
# Drift prevent - Drift prevention slope
self._drift_prevent = 0
if rospy.has_param('~lambda'):
coefs = rospy.get_param('~lambda')
if len(coefs) == 6:
self._lambda = np.array(coefs)
else:
raise rospy.ROSException('lambda coefficients: 6 coefficients '
'needed')
print 'lambda=', self._lambda
if rospy.has_param('~rho_constant'):
coefs = rospy.get_param('~rho_constant')
if len(coefs) == 6:
self._rho_constant = np.array(coefs)
else:
raise rospy.ROSException('rho_constant coefficients: 6 coefficients '
'needed')
print 'rho_constant=', self._rho_constant
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')
print 'k=', self._k
if rospy.has_param('~c'):
coefs = rospy.get_param('~c')
if len(coefs) == 6:
self._c = np.array(coefs)
else:
raise rospy.ROSException('c coefficients: 6 coefficients '
'needed')
print 'c=', self._c
if rospy.has_param('~adapt_slope'):
coefs = rospy.get_param('~adapt_slope')
if len(coefs) == 3:
self._adapt_slope = np.array(coefs)
else:
raise rospy.ROSException('adapt_slope coefficients: 6 coefficients '
'needed')
print 'adapt_slope=', self._adapt_slope
if rospy.has_param('~rho_0'):
coefs = rospy.get_param('~rho_0')
if len(coefs) == 6:
self._rho_0 = np.array(coefs)
else:
raise rospy.ROSException('rho_0 coefficients: 6 coefficients '
'needed')
print 'rho_0=', self._rho_0
if rospy.has_param('~drift_prevent'):
scalar = rospy.get_param('~drift_prevent')
if not isinstance(scalar, list):
self._drift_prevent = scalar
else:
raise rospy.ROSException('drift_prevent needs to be a scalar value')
print 'drift_prevent=', self._drift_prevent
# Enable(1) / disable(0) integral term in the sliding surface
if rospy.has_param('~enable_integral_term'):
self._sliding_int = rospy.get_param('~enable_integral_term')
else:
self._sliding_int = 0
# Enable(1) / disable(0) adaptive uncertainty upper boundaries for
# robust control
if rospy.has_param('~adaptive_bounds'):
self._adaptive_bounds = rospy.get_param('~adaptive_bounds')
else:
self._adaptive_bounds = 1
# Enable(1) / disable(0) constant uncertainty upper boundaries for
# robust control
if rospy.has_param('~constant_bound'):
self._constant_bound = rospy.get_param('~constant_bound')
else:
self._constant_bound = 1
# Enable(1) / disable(0) equivalent control term
if rospy.has_param('~ctrl_eq'):
self._ctrl_eq = rospy.get_param('~ctrl_eq')
else:
self._ctrl_eq = 1
# Enable(1) / disable(0) linear control term
if rospy.has_param('~ctrl_lin'):
self._ctrl_lin = rospy.get_param('~ctrl_lin')
else:
self._ctrl_lin = 1
# Enable(1) / disable(0) robust control term
if rospy.has_param('~ctrl_robust'):
self._ctrl_robust = rospy.get_param('~ctrl_robust')
else:
self._ctrl_robust = 1
# Integrator component
self._int = np.zeros(6)
# Error for the vehicle pose
self._error_pose = np.zeros(6)
# Sliding Surface
self._s_b = np.zeros(6)
# Time derivative of the rotation matrix
self._rotBtoI_dot = np.zeros(shape=(3, 3), dtype=float)
# Linear acceleration estimation
self._accel_linear_estimate_b = np.zeros(3)
# Angular acceleration estimation
self._accel_angular_estimate_b = np.zeros(3)
# Acceleration estimation
self._accel_estimate_b = np.zeros(6)
# adaptive term of uncertainties upper bound estimation
self._rho_adapt = np.zeros(6)
# Upper bound for model uncertainties and disturbances
self._rho_total = np.zeros(6)
# Equivalent control
self._f_eq = np.zeros(6)
# Linear term of controller
self._f_lin = np.zeros(6)
# Robust control
self._f_robust = np.zeros(6)
# Total control
self._tau = np.zeros(6)
self._services['set_mb_sm_controller_params'] = rospy.Service(
'set_mb_sm_controller_params',
SetMBSMControllerParams,
self.set_mb_sm_controller_params_callback)
self._services['get_mb_sm_controller_params'] = rospy.Service(
'get_mb_sm_controller_params',
GetMBSMControllerParams,
self.get_mb_sm_controller_params_callback)
self._is_init = True
self._logger.info(self._LABEL + ' ready')
def __init__(self):
DPControllerBase.__init__(self, is_model_based=False)
self._logger.info('Initializing: ' + self._LABEL)
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._tau = np.zeros(6)
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._is_init = True
self._logger.info(self._LABEL + ' ready')