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)
# 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