Пример #1
0
    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
Пример #2
0
	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]]))
Пример #3
0
    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)
Пример #4
0
    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)
Пример #5
0
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())
Пример #6
0
    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)
Пример #7
0
    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)
Пример #8
0
    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