def calcDiff(self, q, recalc=False): if recalc: self.calc(q) J = np.dot( pin.Jlog3(self.rMf), pin.getFrameJacobian(self.rmodel, self.rdata, self.ee_frame_id, pin.ReferenceFrame.LOCAL)[3:, :]) self.J = self.weight_matrix.dot(J) return self.J
def calcDiff(self, data, x, u): pinocchio.updateFramePlacements(self.state.pinocchio, data.shared.pinocchio) data.rJf = pinocchio.Jlog3(data.rRf) data.fJf = pinocchio.getFrameJacobian(self.state.pinocchio, data.shared.pinocchio, self.Rref.frame, pinocchio.ReferenceFrame.LOCAL)[3:, :] data.J = data.rJf * data.fJf self.activation.calcDiff(data.activation, data.r) data.Rx = np.hstack([data.J, pinocchio.utils.zero((self.activation.nr, self.state.nv))]) data.Lx = np.vstack([data.J.T * data.activation.Ar, pinocchio.utils.zero((self.state.nv, 1))]) data.Lxx = np.vstack([ np.hstack([data.J.T * data.activation.Arr * data.J, pinocchio.utils.zero((self.state.nv, self.state.nv))]), pinocchio.utils.zero((self.state.nv, self.state.ndx)) ])
def calcDiff(self, data, x, u, recalc=True): if recalc: self.calc(data, x, u) nq = self.nq pinocchio.updateFramePlacements(self.pinocchio, data.pinocchio) J = np.dot( pinocchio.Jlog3(data.rRf), pinocchio.getFrameJacobian(self.pinocchio, data.pinocchio, self.frame, pinocchio.ReferenceFrame.LOCAL)[3:, :]) Ax, Axx = self.activation.calcDiff(data.activation, data.residuals, recalc=recalc) data.Rq[:, :nq] = J data.Lq[:] = np.dot(J.T, Ax) data.Lqq[:, :] = np.dot(data.Rq.T, Axx * data.Rq) # J is a matrix, use Rq instead. return data.cost
def test_Jlog3(self): m = eye(3) J = pin.Jlog3(m) self.assertApprox(J, eye(3))