def f(self, x, uu):
u = np.clip(uu, -6, 6)
g = x[0:16].reshape((4, 4))
R, p = TransToRp(g)
omega = x[16:19]
v = x[19:]
twist = x[16:]
# u[0] *= 0.8
F = self.kt * (u[0] + u[1] + u[2] + u[3])
M = np.array([
self.kt * self.arm_length * (u[1] - u[3]),
self.kt * self.arm_length * (u[2] - u[0]),
self.km * (u[0] - u[1] + u[2] - u[3])
])
inertia_dot_omega = np.dot(self.inertia_matrix, omega)
inner_rot = M + cross(inertia_dot_omega, omega)
omegadot = np.dot(self.inv_inertia_matrix,
inner_rot) - self.ang_damping * omega
vdot = self.inv_mass * F * np.array([0., 0., 1.]) - cross(
omega, v) - 9.81 * np.dot(R.T, np.array([0., 0., 1.
])) - self.vel_damping * v
dg = np.dot(g, VecTose3(twist)).ravel()
return np.concatenate((dg, omegadot, vdot))
def get_all_measurement(x): ##eq29 9*1
g = x[0:16].reshape((4, 4)) ## SE(3) matrix
R, p = TransToRp(g)
twist = x[16:] ##select start from 17th
grot = np.dot(R, [0., 0., -9.81]) ## gravity vec related to body frame
return np.concatenate((p, grot, twist))
def get_position(x):
g = x[0:16].reshape((4, 4))
R, p = TransToRp(g)
return p
def get_measurement(x):
g = x[0:16].reshape((4, 4)) ## SE(3) matrix
R, p = TransToRp(g)
twist = x[16:]
grot = np.dot(R, [0., 0., -9.81]) ## gravity vec rel to body frame
return np.concatenate((grot, twist))