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):
P = bMm * dM * mMb * dO
errs.extend(pinocchio.log6(P).vector.tolist())
return np.array(errs)
def compute_rewards(self, achieved_goal, goal, action, collided):
"""
if ||achieved_goal-goal|| < dist_goal, return -d(a_goal, goal) only if not in collision
if ||achieved_goal-goal|| > dist_goal, return free or collision reward
else return previous reward
"""
dist_goal_success = self.robot_props["dist_goal"]
collided = collided.astype(bool)
n_joints = self.robot_n_joints
# links defined goal
achieved_goal = achieved_goal.reshape(-1, n_joints, 4, 4)
goal = goal.reshape(-1, n_joints, 4, 4)
diff = np.linalg.inv(achieved_goal) @ goal
motions = np.zeros((diff.shape[0], n_joints, 6))
for i, d in enumerate(diff):
for j, dj, in enumerate(d):
m = pin.log6(dj)
motions[i, j, :3] = m.linear
motions[i, j, 3:] = m.angular
dist_goal = np.linalg.norm(motions, axis=2)
near_goal = (dist_goal < dist_goal_success).all(1, keepdims=True)
success = np.logical_and(near_goal, ~collided.any(1, keepdims=True))
done = success
reward = np.zeros((achieved_goal.shape[0], 1))
energy = np.linalg.norm(action, axis=1)[:, None]
reward[:] = -energy
reward[~collided] += self.dict_reward["free"]
reward[collided] += self.dict_reward["collision"]
reward[success] += self.dict_reward["goal"]
return reward, done, success
def feedback(self, msg):
if not self._started: return
# compute distance to goal
cur = _PoseToSE3(msg.base_position.pose)
import pinocchio, numpy as np
w = pinocchio.log6(cur.inverse() * self._goal)
if np.linalg.norm(w) < self.distance_thr:
self._next_goal_sent = True
self._send_next_goal()
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)
def calc(self, data, x):
assert (self.Mref.oMf is not None or self.gains[0] == 0.)
data.Jc = pinocchio.getFrameJacobian(self.state.pinocchio, data.pinocchio, self.Mref.frame,
pinocchio.ReferenceFrame.LOCAL)
data.a0 = pinocchio.getFrameAcceleration(self.state.pinocchio, data.pinocchio, self.Mref.frame).vector
if self.gains[0] != 0.:
self.rMf = self.Mref.oMf.inverse() * data.pinocchio.oMf[self.Mref.frame]
data.a0 += np.asscalar(self.gains[0]) * pinocchio.log6(self.rMf).vector
if self.gains[1] != 0.:
v = pinocchio.getFrameVelocity(self.state.pinocchio, data.pinocchio, self.Mref.frame).vector
data.a0 += np.asscalar(self.gains[1]) * v
def represent_goal(self, achieved_goal, desired_goal):
if self.coordinate_frame == "local":
achieved_goal = achieved_goal.reshape(-1, self.n_joints, 4, 4)
desired_goal = desired_goal.reshape(-1, self.n_joints, 4, 4)
diff = np.linalg.inv(achieved_goal) @ desired_goal
motions = np.zeros((diff.shape[0], self.n_joints, 6))
for i, d in enumerate(diff):
for j, dj in enumerate(d):
m = pin.log6(dj)
motions[i, j, :3] = m.linear
motions[i, j, 3:] = m.angular
goal_repr = motions
goal_repr = goal_repr[:, :, :self.goal_rep_dim]
elif self.coordinate_frame == "global":
goal_repr = desired_goal[:, :self.goal_rep_dim]
return goal_repr.reshape(goal_repr.shape[0], -1)
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 plotOdom(mMocap, bMm):
mMb = bMm.inverse()
bMocap = [None if mM is None else mM * mMb for mM in mMocap]
deltas = []
K = 10
for i in range(len(bMocap) - K):
if bMocap[i + K] is None or bMocap[i] is None: continue
deltas.append(
pinocchio.log6(bMocap[i].inverse() * bMocap[i + K]).vector /
(K * dt))
deltas = np.array(deltas)
plt.subplot(2, 1, 1)
plt.plot(deltas[:, 0], label="vx")
plt.plot(deltas[:, 1], label="vy")
plt.plot(deltas[:, 2], label="vz")
plt.legend()
plt.subplot(2, 1, 2)
plt.plot(deltas[:, 3], label="wx")
plt.plot(deltas[:, 4], label="wy")
plt.plot(deltas[:, 5], label="wz")
plt.legend()
plt.show()
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_log6_homogeneous(self):
m = eye(4)
v = pin.log6(m)
self.assertApprox(v.vector, zero(6))
def test_log6(self):
m = pin.SE3.Identity()
v = pin.log6(m)
self.assertApprox(v.vector, zero(6))
def f_fx(self, X, f, fx):
wMt = self.variables.tag(X, self.itag)
tdMt = self.tdMw * wMt
f[:] = pinocchio.log6(tdMt).vector
fx[:] = np.zeros_like(fx)
self.variables.Jtag(fx, self.itag)[:] = pinocchio.Jlog6(tdMt)
def test_log6_homogeneous(self):
m = eye(4)
v = pin.log6(m)
self.assertApprox(v.vector, zero(6))
def test_log6(self):
m = pin.SE3.Identity()
v = pin.log6(m)
self.assertApprox(v.vector, zero(6))
# r = optimize.least_squares(cost.f, x0, jac=cost.jac, method='trf', loss='huber', verbose=2)
nu_c = r.x[:6]
nu_b = r.x[6:]
# recover estimated cst transfo
cm_M_c_est = cm_M_c0 * pin.exp6(nu_c)
bm_M_b_est = bm_M_b0 * pin.exp6(nu_b)
print('cm_M_c_est\n', cm_M_c_est)
print('bm_M_b_est\n', bm_M_b_est)
print('cm_M_c gtr\n', cm_M_c)
print('bm_M_b gtr\n', bm_M_b)
print('cm_M_c_err', pin.log6(cm_M_c_est.inverse() * cm_M_c))
print('bm_M_b_err', pin.log6(bm_M_b_est.inverse() * bm_M_b))
print()
print('r.cost')
print(r.cost)
print()
# residuals RMSE
res = r.fun.reshape((N, 6))
rmse_arr = np.sqrt(np.mean(res**2, axis=0))
# examine the problem jacobian at the solution
J = r.jac
H = J.T @ J
u, s, vh = np.linalg.svd(H, full_matrices=True)
def f(self, X, f):
wMt = self.variables.tag(X, self.itag)
f[:] = pinocchio.log6(self.tdMw * wMt).vector
def computeValueAndJacobian(self):
model = self.robot.model; data = self.robot.data
nj = len(self.joints)
for im, measurement in enumerate(self.measurements):
# ^
# compute configuration q = q + q
# i i off
qi = neutral(model)
for i in range(nj):
imeas = self.measurementIndices[i]
imodel = self.modelQIndices[i]
qi[imodel] = measurement['joint_states'][imeas] + \
self.variable.q_off[i,0]
forwardKinematics(model, data, qi)
computeJointJacobians(model, data)
# position of the head
Th = data.oMi[self.headId]
hTc = self.variable.hTc
if measurement.has_key('left_gripper'):
meas_cTs = measurement['left_gripper']
Tw = data.oMi[self.lwId]
wTs = self.variable.lwTls
Jw_full = getJointJacobian(model, data, self.lwId, \
ReferenceFrame.LOCAL)
left = True
else:
meas_cTs = measurement['right_gripper']
Tw = data.oMi[self.rwId]
wTs = self.variable.rwTrs
Jw_full = getJointJacobian(model, data, self.rwId, \
ReferenceFrame.LOCAL)
left = False
valueSE3 = meas_cTs.inverse()*hTc.inverse()*Th.inverse()*Tw*wTs
value = log6(valueSE3)
Jlog = Jlog6(valueSE3)
self.value[6*im+0:6*im+6] = value.vector.reshape(6,1)
# Compute Jacobian
Xh = Th.toActionMatrix()
Xw_inv = Tw.toActionMatrixInverse()
wXs_inv = wTs.toActionMatrixInverse()
hXc = hTc.toActionMatrix()
sTc = wTs.inverse()*Tw.inverse()*Th*hTc;
sXc = sTc.toActionMatrix()
# Fill reduced Jacobian matrices
Jh_full = getJointJacobian(model, data, self.headId, \
ReferenceFrame.LOCAL)
# columns relative to q_off
for i,c in enumerate (self.modelVIndices):
self.Jh[0:6,i:i+1] = Jh_full[0:6,c:c+1]
self.Jw[0:6,i:i+1] = Jw_full[0:6,c:c+1]
self.jacobian[6*im+0:6*im+6,0:nj] = \
Jlog.dot(wXs_inv).dot(-Xw_inv.dot(Xh).dot(self.Jh) + self.Jw)
# columns relative to hTc
self.jacobian[6*im+0:6*im+6,nj:nj+6] = -Jlog.dot(sXc)
# columns relative to lwTls and rwTrs
if left:
self.jacobian[6*im+0:6*im+6,nj+6: nj+12] = Jlog
self.jacobian[6*im+0:6*im+6,nj+12:nj+18] = np.zeros((6,6))
else:
self.jacobian[6*im+0:6*im+6,nj+6: nj+12] = np.zeros((6,6))
self.jacobian[6*im+0:6*im+6,nj+12:nj+18] = Jlog
