16 from .vehicle
import Vehicle, cross_product_operator
17 from uuv_thrusters
import ThrusterManager
18 from uuv_auv_actuator_interface
import ActuatorManager
21 CASADI_IMPORTED =
True 23 CASADI_IMPORTED =
False 28 Vehicle.__init__(self, inertial_frame_id)
33 self.
eta = casadi.SX.sym(
'eta', 6)
35 self.
nu = casadi.SX.sym(
'nu', 6)
41 casadi.mtimes(self._Mtotal[0:3, 0:3], self.
nu[0:3]) +
42 casadi.mtimes(self._Mtotal[0:3, 3:6], self.
nu[3:6]))
44 casadi.mtimes(self._Mtotal[3:6, 0:3], self.
nu[0:3]) +
45 casadi.mtimes(self._Mtotal[3:6, 3:6], self.
nu[3:6]))
52 self.
DMatrix = - casadi.diag(self._linear_damping)
53 self.
DMatrix -= casadi.diag(self._linear_damping_forward_speed)
54 self.
DMatrix -= casadi.diag(self._quad_damping * self.
nu)
57 Rx = np.array([[1, 0, 0],
58 [0, casadi.cos(self.
eta[3]), -1 * casadi.sin(self.
eta[3])],
59 [0, casadi.sin(self.
eta[3]), casadi.cos(self.
eta[3])]])
60 Ry = np.array([[casadi.cos(self.
eta[4]), 0, casadi.sin(self.
eta[4])],
62 [-1 * casadi.sin(self.
eta[4]), 0, casadi.cos(self.
eta[4])]])
63 Rz = np.array([[casadi.cos(self.
eta[5]), -1 * casadi.sin(self.
eta[5]), 0],
64 [casadi.sin(self.
eta[5]), casadi.cos(self.
eta[5]), 0],
67 R_n_to_b = casadi.transpose(casadi.mtimes(Rz, casadi.mtimes(Ry, Rx)))
69 if inertial_frame_id ==
'world_ned':
70 Fg = casadi.SX([0, 0, -self.mass * self.gravity])
71 Fb = casadi.SX([0, 0, self.volume * self.gravity * self.density])
73 Fg = casadi.SX([0, 0, self.mass * self.gravity])
74 Fb = casadi.SX([0, 0, -self.volume * self.gravity * self.density])
76 self.
gVec = casadi.SX.zeros(6)
78 self.
gVec[0:3] = -1 * casadi.mtimes(R_n_to_b, Fg + Fb)
79 self.
gVec[3:6] = -1 * casadi.mtimes(
80 R_n_to_b, casadi.cross(self._cog, Fg) + casadi.cross(self._cob, Fb))
83 T = 1 / casadi.cos(self.
eta[4]) * np.array(
84 [[0, casadi.sin(self.
eta[3]) * casadi.sin(self.
eta[4]), casadi.cos(self.
eta[3]) * casadi.sin(self.
eta[4])],
85 [0, casadi.cos(self.
eta[3]) * casadi.cos(self.
eta[4]), -casadi.cos(self.
eta[4]) * casadi.sin(self.
eta[3])],
86 [0, casadi.sin(self.
eta[3]), casadi.cos(self.
eta[3])]])
89 casadi.mtimes(casadi.transpose(R_n_to_b), self.
nu[0:3]),
90 casadi.mtimes(T, self.
nu[3::]))
92 self.
u = casadi.SX.sym(
'u', 6)
eta
Generalized position vector.
nu
Generalized velocity vector.
def __init__(self, inertial_frame_id='world')
def cross_product_operator(x)