def test_exp6(self):
v = pin.Motion.Zero()
M = pin.exp6(v)
self.assertTrue(M.isIdentity())
M2 = pin.exp6(np.array(v))
self.assertTrue(M2.isIdentity())
def jacobian_fd(dMocap, dOdom, bMm, eps=0.001):
dM = np.zeros((6))
err = error(dMocap, dOdom, bMm)
J = np.zeros((err.shape[0], 6))
for i in range(6):
dM[i] = eps
bMm2 = bMm * pinocchio.exp6(dM)
err2 = error(dMocap, dOdom, bMm2)
J[:, i] = (err2 - err) / eps
dM[i] = 0.
return J
def f(self, x):
self.x_arr.append(x)
nu_c = x[:
6] # currently estimated transfo as se3 6D vector representation
nu_b = x[
6:] # currently estimated transfo as se3 6D vector representation
cm_M_c = self.cm_M_c0 * pin.exp6(
nu_c) # currently estimated transfo as SE3 Lie group
bm_M_b = self.cm_M_c0 * pin.exp6(
nu_b) # currently estimated transfo as SE3 Lie group
b_M_bm = bm_M_b.inverse()
res = np.zeros(self.N_res)
for i in range(self.N):
bm_M_cm = self.bm_M_cm_traj[i]
c_M_b = self.c_M_b_cosy_traj[i]
res[6 * i:6 * i + 6] = pin.log6(bm_M_cm * cm_M_c * c_M_b *
b_M_bm).np
self.cost_arr.append(np.linalg.norm(res))
return res
def estimate(transforms, Tm):
errs = [ pinocchio.log6(Tm.actInv(T)).vector for T in transforms ]
v_mean = np.mean(errs, 0)
v_var = np.var(errs, 0)
Tm = Tm * pinocchio.exp6(pinocchio.Motion(v_mean))
return Tm, v_var
def test_exp6(self):
v = pin.Motion.Zero()
m = pin.exp6(v)
self.assertTrue(m.isIdentity())
def test_exp6(self):
v = pin.Motion.Zero()
m = pin.exp6(v)
self.assertTrue(m.isIdentity())
if __name__ == '__main__':
N = 1000
dt = 0.01
print('Tot seconds ', N * dt)
t = np.arange(N)
A = 0.5 * (np.random.random(6) - 0.5)
f = 0.2 * (np.random.random(6) - 0.5)
ft_arr = np.outer(t, f)
nu_traj = A * np.sin(2 * np.pi * ft_arr)
# nu_traj = A*(np.random.random((N,6)) - 0.5)
m_M_c_traj = [pin.exp6(nu) for nu in nu_traj] # moving camera
m_M_b_traj = [pin.SE3.Identity() for _ in range(N)] # resting object
# cozy pose measurements
c_M_b_cosy_traj = [
m_M_c.inverse() * m_M_b
for m_M_c, m_M_b in zip(m_M_c_traj, m_M_b_traj)
]
# constant transformation to estimate
cm_M_c = pin.SE3.Random()
bm_M_b = pin.SE3.Random()
c_M_cm = cm_M_c.inverse()
b_M_bm = bm_M_b.inverse()
# mocap measurements
def calibrate(measurements):
""" compute maMmo """
def err_jac(maMmo):
err = []
jac = []
for maMb, moMb in measurements:
M = maMb.inverse() * maMmo * moMb
err.extend(pinocchio.log6(M).vector.tolist())
jac.extend(
np.dot(pinocchio.Jlog6(M),
moMb.toActionMatrixInverse()).tolist())
return np.array(err), np.array(jac)
maMmo = pinocchio.SE3.Identity()
iter = 100
ethr = 0.001
Jthr = 0.001
mthr = 1e-4
def norm2(a):
return np.sum(a**2)
from numpy.linalg import norm
while iter > 0:
err, J = err_jac(maMmo)
els = norm2(err)
if norm(err) < ethr:
print("Error is very small")
break
if norm(J) < Jthr:
print("Jacobian is very small")
break
d, res, rank, s = np.linalg.lstsq(J, -err)
# do line search on els = norm2(err), Jls = 2 * err^T * J
# els(u) = norm2(err(q + u*d)) ~ els(0) + u * Jls * d
Jls = 2 * np.dot(err, J)
m = np.dot(Jls, d)
if abs(m) < mthr:
print("m is very small.", m)
break
assert m < 0, str(m) + " should be negative"
alpha = 1.
c = 0.1 # factor for the linear part
rate = 0.5
alphaDefault = None
while True:
maMmo2 = maMmo * pinocchio.exp6(alpha * d)
err2, _ = err_jac(maMmo2)
els2 = norm2(err2)
if els2 < els + c * alpha * m:
break
if alphaDefault is None and els2 < els:
alphaDefault = alpha
alpha *= rate
if alpha < 1e-5:
if alphaDefault is None:
print(
"failed to find a alpha that makes the error decrease. m =",
m)
return maMmo
print("failed to find correct alpha")
alpha = alphaDefault
maMmo2 = maMmo * pinocchio.exp6(alpha * d)
break
if iter % 10 == 0:
print("{:4} {:^8} {:^8} {:^8} {:^8}".format(
"iter", "err", "J", "d", "alpha"))
print("{:4} {:8.5} {:8.5} {:8.5} {:8.5}".format(
iter, np.sqrt(els2), norm(J), norm(d), alpha))
maMmo = maMmo2
iter -= 1
return maMmo
def optimize(
dMocap,
dOdom,
bMm=pinocchio.SE3.Identity(),
iter=100,
ethr=0.001,
Jthr=0.001,
mthr=1e-4,
fd=False,
):
def norm2(a):
return np.sum(a**2)
from numpy.linalg import norm
err2 = None
while iter > 0:
err = error(dMocap, dOdom, bMm) if err2 is None else err2
els = norm2(err) if err2 is None else els2
if norm(err) < ethr:
print("Error is very small")
break
if fd:
J = jacobian_fd(dMocap, dOdom, bMm)
else:
J = jacobian(dMocap, dOdom, bMm)
if norm(J) < Jthr:
print("Jacobian is very small")
break
d, res, rank, s = np.linalg.lstsq(J, -err)
# do line search on els = norm2(err), Jls = 2 * err^T * J
# els(u) = norm2(err(q + u*d)) ~ els(0) + u * Jls * d
Jls = 2 * np.dot(err, J)
m = np.dot(Jls, d)
if abs(m) < mthr:
print("m is very small.", m)
break
assert m < 0, str(m) + " should be negative"
alpha = 1.
c = 0.1 # factor for the linear part
rate = 0.5
alphaDefault = None
while True:
bMm2 = bMm * pinocchio.exp6(alpha * d)
err2 = error(dMocap, dOdom, bMm2)
els2 = norm2(err2)
if els2 < els + c * alpha * m:
break
if alphaDefault is None and els2 < els:
alphaDefault = alpha
alpha *= rate
if alpha < 1e-5:
if alphaDefault is None:
print(
"failed to find a alpha that makes the error decrease. m =",
m)
return bMm
print("failed to find correct alpha")
alpha = alphaDefault
bMm2 = bMm * pinocchio.exp6(alpha * d)
err2 = error(dMocap, dOdom, bMm2)
els2 = norm2(err2)
break
if iter % 10 == 0:
print("{:4} {:^8} {:^8} {:^8} {:^8}".format(
"iter", "err", "J", "d", "alpha"))
print("{:4} {:8.5} {:8.5} {:8.5} {:8.5}".format(
iter, np.sqrt(els2), norm(J), norm(d), alpha))
#bMm = bMm * pinocchio.exp6(d)
bMm = bMm2
iter -= 1
return bMm
v2 = np.array([0.1, 0., 0., 0., 0., 0.3])
dt = 0.01
xReg = 1.
K = 5
dMocap = []
dOdom = []
for wMm, v in [
(d0se3, v0),
(d1se3, v1),
(d2se3, v2),
]:
for i in range(len(wMm) - K):
if wMm[i + K] is None or wMm[i] is None: continue
dMocap.append(wMm[i + K].inverse() * wMm[i])
dOdom.append(pinocchio.exp6(K * v * dt))
subsample = 10
if subsample > 1:
dMocap = dMocap[::10]
dOdom = dOdom[::10]
def error(dMocap, dOdom, bMm):
# x regularisation
if xReg > 0:
errs = (xReg * pinocchio.log6(bMm).vector).tolist()[2:5]
else:
errs = []
assert len(dMocap) == len(dOdom)
mMb = bMm.inverse()
for dM, dO in zip(dMocap, dOdom):