def v(w): l = w[6] w = w[:3, :] if len(l.free_symbols) == 0 and abs(l.evalf()) < 1e-9: return se3.v(w[:3, :]) s = skew(w) e = sy.exp(l) theta = norm3(w) l2t2 = l**2 + theta**2 A = (e - 1) / l if len(theta.free_symbols) == 0 and abs(theta.evalf()) < 1e-9: B = (1 + e * (l - 1)) / l**2 C = (-2 + e * (2 - 2 * l + l**2)) / (2 * l**3) else: B = (theta * (1 - e * sy.cos(theta)) + e * l * sy.sin(theta)) / (theta * l2t2) C = A / (theta**2) - e * sy.sin(theta) / (theta * l2t2) - l * ( e * sy.cos(theta) - 1) / (theta**2 * l2t2) return A * sy.eye(3) + B * s + C * s * s
def dw_dw(w): s = skew(w) theta = norm3(w) A = -1 / 2 B = 1 / theta**2 - (1 + sy.cos(theta)) / (2 * theta * sy.sin(theta)) return sy.eye(3) + A * s + B * s * s
def exp(w): s = skew(w) theta = norm3(w) if len(theta.free_symbols) == 0 and theta.evalf() < 1e-9: A = 1 B = 1 / 2 else: A = sy.sin(theta) / theta B = (1 - sy.cos(theta)) / theta**2 return sy.eye(3) + A * s + B * s * s
def v(w): w = w[:3, :] s = skew(w) theta = norm3(w) if len(theta.free_symbols) == 0 and theta.evalf() < 1e-9: A = 1 / 2 B = 1 / 6 else: A = (1 - sy.cos(theta)) / theta**2 B = (theta - sy.sin(theta)) / theta**3 return sy.eye(3) + A * s + B * s * s
def dv(w, dw_dx): w = w[:3, :] dw_dx = dw_dx[:3, :] wW = (w.T * dw_dx)[0, 0] s = skew(w) S = skew(dw_dx) theta = norm3(w) F1 = sy.sin(theta) / theta**3 - 2 * (1 - sy.cos(theta)) / theta**4 F2 = (1 - sy.cos(theta)) / theta**4 - 3 * (theta - sy.sin(theta)) / theta**5 F3 = (1 - sy.cos(theta)) / theta**2 F4 = (theta - sy.sin(theta)) / theta**3 return wW * (F1 * s + F2 * s * s) + F3 * S + F4 * (s * S + S * s)
def dv(w, dw_dx): l = w[6] w = w[:3, :] if len(l.free_symbols) == 0 and abs(l.evalf()) < 1e-9: return se3.dv(w, dw_dx) dw_dx = dw_dx[:3, :] wW = (w.T * dw_dx)[0, 0] s = skew(w) S = skew(dw_dx) e = sy.exp(l) theta = norm3(w) l2t2 = l**2 + theta**2 e = sy.exp(l) le1cos = l * (1 - e * sy.cos(theta)) F1 = -2*(l*e*sy.sin(theta)) / (theta*l2t2**2) - \ 2*(1 - e*sy.cos(theta)) / (l2t2**2) + \ (l*e*sy.cos(theta) - e*sy.cos(theta) + 1) / (theta**2 * l2t2) + \ (e*sy.sin(theta)) / (theta * l2t2) - \ l*e*sy.sin(theta) / (theta**3 * l2t2) - \ (1 - e*sy.cos(theta)) / (theta**2*l2t2) F2 = -2*le1cos / (theta ** 2 * l2t2 ** 2) - \ 2*le1cos / (theta**4 * l2t2) + \ 2*e*sy.sin(theta) / (theta * l2t2**2) - \ e*sy.cos(theta) / (theta**2 * l2t2) + \ l*e*sy.sin(theta) / (theta**3 * l2t2) + \ e*sy.sin(theta) / (theta**3 * l2t2) - \ 2*(e-1) / (l*theta**4) F3 = l*e*sy.sin(theta) / (theta * l2t2) + \ (1 - e*sy.cos(theta)) / l2t2 F4 = le1cos / (theta**2 * l2t2) - \ e * sy.sin(theta) / (theta * l2t2) - \ (-e + 1) / (l * theta**2) return wW * (F1 * s + F2 * s * s) + F3 * S + F4 * (s * S + S * s)
def dv_dlambda(w): l = w[6] w = w[:3, :] s = skew(w) e = sy.exp(l) theta = norm3(w) l2t2 = l**2 + theta**2 if len(l.free_symbols) == 0 and abs(l.evalf()) < 1e-9: A = 1 / 2 if len(theta.free_symbols) == 0 and theta.evalf() < 1e-9: B = 1 / 3 C = 1 / 8 else: B = -(theta * sy.cos(theta) - sy.sin(theta)) / theta**3 C = -(-theta**2 + 2 * theta * sy.sin(theta) + 2 * sy.cos(theta) - 2) / (2 * theta**4) else: A = e / l - (e - 1) / l**2 if len(theta.free_symbols) == 0 and theta.evalf() < 1e-9: B = (l**2 * e - 2 * l * e + 2 * e - 2) / l**3 C = (l**3 * e - 3 * l**2 * e + 6 * l * e - 6 * e + 6) / (2 * l**4) else: B = -2*l*(l*e*sy.sin(theta) + theta*(-e*sy.cos(theta) + 1))/(theta*(l2t2)**2) + \ (l*e*sy.sin(theta) - theta*e*sy.cos(theta) + e*sy.sin(theta))/(theta*(l2t2)) C = 2*l**2*(e*sy.cos(theta) - 1)/(theta**2*(l2t2)**2) + \ 2*l*e*sy.sin(theta)/(theta*(l2t2)**2) - \ l*e*sy.cos(theta)/(theta**2*(l2t2)) - \ e*sy.sin(theta)/(theta*(l2t2)) - \ (e*sy.cos(theta) - 1)/(theta**2*(l2t2)) + \ e/(l*theta**2) - \ (e - 1)/(l**2*theta**2) return A * sy.eye(3) + B * s + C * s * s