18 from uuv_control_interfaces
import DPControllerBase
23 PD controller with compensation of restoring forces for the dynamic 27 _LABEL =
'PD controller with compensation of restoring forces' 30 DPControllerBase.__init__(self, is_model_based=
True)
31 self._logger.info(
'Initializing: ' + self.
_LABEL)
33 self.
_Kp = np.zeros(shape=(6, 6))
35 self.
_Kd = np.zeros(shape=(6, 6))
41 if rospy.has_param(
'~Kp'):
42 Kp_diag = rospy.get_param(
'~Kp')
44 self.
_Kp = np.diag(Kp_diag)
46 raise rospy.ROSException(
'Kp matrix error: 6 coefficients ' 49 self._logger.info(
'Kp=' + str([self.
_Kp[i, i]
for i
in range(6)]))
51 if rospy.has_param(
'~Kd'):
52 Kd_diag = rospy.get_param(
'~Kd')
54 self.
_Kd = np.diag(Kd_diag)
56 raise rospy.ROSException(
'Kd matrix error: 6 coefficients ' 59 self._logger.info(
'Kd=' + str([self.
_Kd[i, i]
for i
in range(6)]))
62 self._logger.info(self.
_LABEL +
' ready')
72 self._vehicle_model._update_restoring(use_sname=
True)
74 self.
_tau = np.dot(self.
_Kp, self.error_pose_euler) \
75 + np.dot(self.
_Kd, self._errors[
'vel']) \
76 + self._vehicle_model.restoring_forces
78 self.publish_control_wrench(self.
_tau)
80 if __name__ ==
'__main__':
81 print(
'Starting PD controller with compensation of restoring forces')
82 rospy.init_node(
'rov_pd_grav_compensation_controller')
87 except rospy.ROSInterruptException:
88 print(
'caught exception')
def update_controller(self)
def _reset_controller(self)