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