def getOdomJacobians(self, ab, wb, dt):
e3 = np.array([0, 0, 1])
F = np.zeros((9, 9))
F[:3, 3:6] = self.q_i_from_b.R.T
F[:3, 6:] = self.q_i_from_b.R.T @ skew(self.velocity)
F[3:6, 3:6] = -skew(wb)
F[3:6, 6:] = skew(self.q_i_from_b.R @ (self.g * e3))
F[6:, 6:] = -skew(wb)
G = np.zeros((9, 6))
G[3:6, 2] = e3
G[3:6, 3:] = -skew(self.velocity)
G[6:, 3:] = -np.eye(3)
return F, G
def getValuesFromPose(self, P): '''return the virtual values of the pots corresponding to the pose P''' vals = [] grads = [] for i, r, l, placement, attach_p in zip(range(3), self.rs, self.ls, self.placements, self.attach_ps): #first pot axis a = placement.rot * col([1, 0, 0]) #second pot axis b = placement.rot * col([0, 1, 0]) #string axis c = placement.rot * col([0, 0, 1]) #attach point on the joystick p_joystick = P * attach_p v = p_joystick - placement.trans va = v - dot(v, a)*a vb = v - dot(v, b)*b #angles of the pots alpha = math.atan2(dot(vb, a), dot(vb, c)) beta = math.atan2(dot(va, b), dot(va, c)) vals.append(alpha) vals.append(beta) #calculation of the derivatives dv = np.bmat([-P.rot.mat() * quat.skew(attach_p), P.rot.mat()]) dva = (np.eye(3) - a*a.T) * dv dvb = (np.eye(3) - b*b.T) * dv dalpha = (1/dot(vb,vb)) * (dot(vb,c) * a.T - dot(vb,a) * c.T) * dvb dbeta = (1/dot(va,va)) * (dot(va,c) * b.T - dot(va,b) * c.T) * dva grads.append(dalpha) grads.append(dbeta) return (col(vals), np.bmat([[grads]]))
def test_right_jacobian_of_transp(self):
for T in self.transforms:
v = np.random.uniform(-10, 10, size=3)
vp, Jr = T.transp(v, Jr=np.eye(6))
Jr_true = np.block([-np.eye(3), skew(vp)])
np.testing.assert_allclose(Jr_true, Jr, atol=1e-10)
def test_left_jacobian_of_transp(self):
for T in self.transforms:
v = np.random.uniform(-10, 10, size=3)
vp, Jl = T.transp(v, Jl=np.eye(6))
vx = skew(v)
Jl_true = np.block([-T.R, T.R @ vx])
np.testing.assert_allclose(Jl_true, Jl)
def mt(v): r = v[0:3] t = v[3:6] x = np.linalg.norm(r) b = trig.cosox2(x) c = trig.sinox3(x) g = trig.specialFun1(x) h = trig.specialFun3(x) I = np.identity(3) return b*quat.skew(t) + c*(r*t.transpose() + t*r.transpose()) + v3.dot(r,t)*((c - b) * I + g*quat.skew(r) + h*r*r.transpose())
def test_right_jacobian_of_rotation(self):
for i in range(100):
q = Quaternion.random()
v = np.random.uniform(-10, 10, size=3)
vp, Jr = q.rota(v, Jr=np.eye(3))
Jr_true = -q.R.T @ skew(v) # James equation missing a transpose.
# Confirmed by testing against numerical differentiation
np.testing.assert_allclose(Jr_true, Jr)
def test_right_jacobian_of_rotp(self):
for i in range(100):
q = Quaternion.random()
v = np.random.uniform(-10, 10, size=3)
vp, Jr = q.rotp(v, Jr=np.eye(3))
vx = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]],
[-v[1], v[0], 0]])
Jr_true = skew(q.R @ v) # left jacobian in james table...?
np.testing.assert_allclose(Jr_true, Jr, atol=1e-10)
def propogateDynamics(self, ab, wb, dt):
e3 = np.array([0, 0, 1])
xdot = self.q_i_from_b.rota(self.velocity)
vdot = skew(self.velocity) @ wb - self.q_i_from_b.rotp(
self.g * e3) + ab[2] * e3
F, G = self.getOdomJacobians(ab, wb, dt)
# Propagate state
dx = np.block([xdot, vdot, wb]) * dt
self.boxplusr(dx)
# Discretize matrices
Fd = np.eye(dx.size) + F * dt + F @ F * (dt**2) / 2
Gd = G * dt
# Propagate Uncertainy
R = spl.block_diag(R_accel, R_gyro)
# self.P_ = F @ self.P_ @ F.T + Q + G @ R @ G.T
# self.P_ = Fd @ self.P_ @ Fd.T + Q*dt**2 + Gd @ R @ Gd.T
self.P_ = Fd @ self.P_ @ Fd.T + Q + Gd @ R @ Gd.T