def testGroupAction(self):
for i in range(100):
rot1 = Rotation.random().as_matrix()
rot2 = Rotation.random().as_matrix()
R1 = SO3(rot1)
R2 = SO3(rot2)
R3 = R1 * R2
R3_true = rot1 @ rot2
np.testing.assert_allclose(R3_true, R3.arr)
def testInv(self):
for i in range(100):
mat = Rotation.random().as_matrix()
R = SO3(mat)
R_inv = R.inv()
R_T = R.transpose()
R_inv_true = np.linalg.inv(mat)
np.testing.assert_allclose(R_inv_true, R_inv.R)
np.testing.assert_allclose(R_inv_true, R_T.R)
def testAdjoint(self):
for i in range(100):
delta = np.random.uniform(-np.pi, np.pi, size=3)
rot = Rotation.random().as_matrix()
R = SO3(rot)
Adj_R = R.Adj
T_true = R * SO3.Exp(delta)
T = SO3.Exp(Adj_R @ delta) * R
np.testing.assert_allclose(T_true.R, T.R)
def testRotatingVector(self):
for i in range(100): #Active rotation
rot1 = Rotation.random().as_matrix()
R = SO3(rot1)
pt = np.random.uniform(-5, 5, size=3)
rot_pt = R.rota(pt)
rot_pt_true = rot1 @ pt
np.testing.assert_allclose(rot_pt_true, rot_pt)
for i in range(100):
rot1 = Rotation.random().as_matrix()
R = SO3(rot1)
pt = np.random.uniform(-5, 5, size=3)
rot_pt = R.rotp(pt)
rot_pt_true = rot1.T @ pt
np.testing.assert_allclose(rot_pt_true, rot_pt)
def testLog(self): #This has issues sometimes. It is infrequent though
for i in range(100):
temp = Rotation.random().as_matrix()
R = SO3(temp)
logR = SO3.log(R)
logR_true = sp.linalg.logm(temp)
if np.linalg.norm(logR_true - logR, ord='fro') > 1e-3:
Pdb().set_trace()
debug = 1
temp = SO3.log(R)
np.testing.assert_allclose(logR_true, logR, atol=1e-10)
def testConstructor(self):
for i in range(100):
angles = np.random.uniform(-np.pi, np.pi, size=3)
R_ex = SO3.fromRPY(angles)
cp = np.cos(angles[0])
sp = np.sin(angles[0])
R1 = np.array([[1, 0, 0], [0, cp, -sp], [0, sp, cp]])
ct = np.cos(angles[1])
st = np.sin(angles[1])
R2 = np.array([[ct, 0, st], [0, 1, 0], [-st, 0, ct]])
cps = np.cos(angles[2])
sps = np.sin(angles[2])
R3 = np.array([[cps, -sps, 0], [sps, cps, 0], [0, 0, 1]])
R_true = Rotation.from_euler(
'ZYX', [angles[2], angles[1], angles[0]]).as_matrix()
R_ex2 = SO3(R_true)
np.testing.assert_allclose(R_true, R_ex.arr)
np.testing.assert_allclose(R_true, R_ex2.arr)
def update(self, state, des):
p = state[0:3]
v = state[3:6]
R_bi = Quat2Rotation(state[6:10])
om_b = state[10:]
e3 = np.array([[0., 0., 1.]]).T
p_err = p - des.pd
pdot_err = v - des.pd_dot
self.p_err_int += self.ts / 2.0 * (p_err + self.p_err_prev)
self.p_err_prev = p_err
et = np.vstack([p_err, pdot_err, self.p_err_int])
# et = np.vstack([p_err, pdot_err])
if not self.clipped_props:
psid = des.psid
psid_dot = des.psid_dot
else:
euler = Quat2Euler(state[6:10])
psid = euler.item(2)
psid_dot = om_b[-1]
sd = np.array([[np.cos(psid)], [np.sin(psid)], [0]])
sd_dot = psid_dot * np.array([[-np.sin(psid)], [np.cos(psid)], [0]])
f = -self.Kt @ et + self.m * (self.g * e3 - des.pd_ddot)
# T = -np.dot(f.T, R_bi.T@e3).item(0)
T = np.linalg.norm(f)
k_d = f / np.linalg.norm(f)
kxs = cross(k_d, sd)
j_d = kxs / np.linalg.norm(kxs)
i_d = cross(j_d, k_d)
R_di = np.hstack([i_d, j_d, k_d])
k_d_dot = proj(k_d) / np.linalg.norm(f) @ (
-self.m * des.pd_dddot -
self.Kt @ (self.At - self.Bt @ self.Kt) @ et)
j_d_dot = proj(j_d) / np.linalg.norm(kxs) @ (cross(k_d, sd_dot) +
cross(k_d_dot, sd))
i_d_dot = cross(j_d, k_d_dot) + cross(j_d_dot, k_d)
R_di_dot = np.hstack([i_d_dot, j_d_dot, k_d_dot])
om_d = vee(R_di.T @ R_di_dot)
#have to numerically differentiate because analytical derivative is nasty
self.om_d_dot = (
(2 * self.sig - self.ts) /
(2 * self.sig + self.ts)) * self.om_d_dot + (
2 / (2 * self.sig + self.ts)) * (om_d - self.om_d_prev)
self.om_d_prev = om_d
R_db = R_bi.T @ R_di
om_err = R_db @ om_d - om_b
r_err = np.array([SO3(R_db).Log().tolist()]).T
om_db_dot = -cross(om_b, R_db @ om_d) + R_db @ self.om_d_dot
if self.control_type == "lee1":
tau = cross(
om_b, self.J @ om_b) + self.J @ om_db_dot + self.kr_lee1 * vee(
skew_sym(R_db)) + self.Kom @ om_err #this uses trace error
elif self.control_type == "lee2":
tau = cross(
R_db @ om_d, self.J @ R_db @ om_d
) + self.J @ R_db @ self.om_d_dot + self.kr_lee2 * vee(
skew_sym(R_db)) / np.sqrt(1.0 + np.trace(
R_db)) + self.Kom @ om_err #this uses wierd trace error
elif self.control_type == "lee3":
r_1d = R_di[:, [0]]
r_2d = R_di[:, [1]]
r_3d = R_di[:, [2]]
r_1b = R_bi[:, [0]]
r_2b = R_bi[:, [1]]
e_r1 = cross(R_bi.T @ r_1d, np.array([[1, 0, 0]]).T)
e_r2 = cross(R_bi.T @ r_2d, np.array([[0, 1, 0]]).T)
Psi_n1 = 1 - (r_1b.T @ r_1d).item(0)
Psi_n2 = 1 - (r_2b.T @ r_2d).item(0)
Psi_e1 = self.alpha_lee3 + self.beta_lee3 * (r_1b.T @ r_3d).item(0)
Psi_e2 = self.alpha_lee3 + self.beta_lee3 * (r_2b.T @ r_3d).item(0)
Psi_1 = self.kr1_lee3 * Psi_n1 + self.kr2_lee3 * Psi_n2
Psi_2 = self.kr1_lee3 * Psi_n1 + self.kr2_lee3 * Psi_e2
Psi_3 = self.kr1_lee3 * Psi_e1 + self.kr2_lee3 * Psi_n2
Psi_choices = [Psi_1, Psi_2, Psi_3]
Psi_min = np.argmin(Psi_choices)
rho = Psi_choices[Psi_min]
if Psi_choices[
self.m_lee3] - rho >= self.delta_lee3 and np.linalg.norm(
om_err) < self.om_lim_lee3:
self.m_lee3 = Psi_min
e_h1 = e_r1 if self.m_lee3 != 2 else -self.beta_lee3 * cross(
R_bi.T @ r_3d,
np.array([[1, 0, 0]]).T)
e_h2 = e_r2 if self.m_lee3 != 1 else -self.beta_lee3 * cross(
R_bi.T @ r_3d,
np.array([[0, 1, 0]]).T)
e_h = self.kr1_lee3 * e_h1 + self.kr2_lee3 * e_h2
tau = cross(
R_db @ om_d, self.J @ R_db @ om_d
) + self.J @ R_db @ self.om_d_dot - e_h + self.Kom_lee3 @ om_err # this uses the overly complicated global hybrid controller
elif self.control_type == "ours":
tau = cross(om_b, self.J @ om_b) + self.J @ om_db_dot + SO3.Jl_inv(
r_err
).T @ self.kr @ r_err + self.Kom @ om_err #this uses log error
commanded_state = np.vstack(
[des.pd, des.pd_dot, Rotation2Quat(R_di), om_d])
delta_unsat = self.mix_inv @ np.vstack([T, tau])
return self.sat(delta_unsat), commanded_state
def calcStep(self, t):
w = np.pi/15.0
g = 9.81
if self._is_circle:
# Complete a circle in 30s
# Flat inputs
x = 3 * np.cos(w*t)
y = 3 * np.sin(w*t)
z = -5.0
psi = 0.0
# World frames
vx = -3 * w * np.sin(w*t)
vy = 3 * w * np.cos(w*t)
vz = 0
# Accelerations in the world frame
ax = -3 * w**2 * np.cos(w*t)
ay = -3 * w**2 * np.sin(w*t)
az = 0
# Jerk
jx = 3 * w**3 * np.sin(w*t)
jy = -3 * w**3 * np.cos(w*t)
jz = 0
ad = np.array([jx, jy, jz])
# Get rotation
t = np.array([ax, ay, az + g])
R_i_from_b = self.getRotation(t, psi)
# Get angular rates
u1 = self._quad_params.mass * np.linalg.norm(t)
zB = R_i_from_b[:,-1]
hw = self._quad_params.mass/u1 * (ad - (zB @ ad)*zB)
p = -hw @ R_i_from_b[:,1]
q = hw @ R_i_from_b[:,0]
r = 0 # Because psi_dot is 0
else:
# Complete Figure 8 in 30s
x = 3 * np.cos(w*t)
y = 3 * np.cos(w*t) * np.sin(w*t)
z = -5.0
psi = 0.0
vx = -3 * w * np.sin(w*t)
vy = -3 * w * (np.cos(w*t)**2 - np.sin(w*t)**2)
vz = 0.0
ax = -3 * w**2 * np.cos(w*t)
ay = 12 * w**2 * np.cos(w*t) * np.sin(w*t)
az = 0.0
jx = 3 * w**3 * np.sin(w*t)
jy = 12 * w**3 * (np.cos(w*t)**2 - np.sin(w*t)**2)
jz = 0
ad = np.array([jx, jy, jz])
t = np.array([ax, ay, az+g])
R_i_from_b = self.getRotation(t, psi)
# Get angular rates
u1 = self._quad_params.mass * np.linalg.norm(t)
zB = R_i_from_b[:,-1]
hw = self._quad_params.mass/u1 * (ad - (zB @ ad)*zB)
p = -hw @ R_i_from_b[:,1]
q = hw @ R_i_from_b[:,0]
r = 0 # Because psi_dot is 0
# return np.array([x, y, z]), np.array([vx, vy, vz]), [email protected]([ax, ay, az+g]), SO3(R_i_from_b), np.array([p, q, r])
return np.array([x, y, z]), [email protected]([vx, vy, vz]), [email protected]([ax, ay, az+g]), SO3(R_i_from_b), np.array([p, q, r])