def Jdiff(self, x1, x2, firstsecond='both'): """ Compute the partial derivatives for diff operator using Pinocchio. :param x1: current state :param x2: next state :param firstsecond: desired partial derivative :return the partial derivative(s) of the diff(x1,x2) function """ assert (firstsecond in ['first', 'second', 'both']) if firstsecond == 'both': return [self.Jdiff(x1, x2, 'first'), self.Jdiff(x1, x2, 'second')] if firstsecond == 'second': dx = self.diff(x1, x2) q = a2m(x1[:self.model.nq]) dq = a2m(dx[:self.model.nv]) Jdq = pinocchio.dIntegrate(self.model, q, dq)[1] return block_diag(np.asarray(np.linalg.inv(Jdq)), np.eye(self.model.nv)) else: dx = self.diff(x2, x1) q = a2m(x2[:self.model.nq]) dq = a2m(dx[:self.model.nv]) Jdq = pinocchio.dIntegrate(self.model, q, dq)[1] return -block_diag(np.asarray(np.linalg.inv(Jdq)), np.eye(self.model.nv))
def Jdiff(self, x1, x2, firstsecond=crocoddyl.Jcomponent.both): if firstsecond == crocoddyl.Jcomponent.both: return [self.Jdiff(x1, x2, crocoddyl.Jcomponent.first), self.Jdiff(x1, x2, crocoddyl.Jcomponent.second)] if firstsecond == crocoddyl.Jcomponent.first: dx = self.diff(x2, x1) q = x2[:self.model.nq] dq = dx[:self.model.nv] Jdq = pinocchio.dIntegrate(self.model, q, dq)[1] return np.matrix(-scl.block_diag(np.linalg.inv(Jdq), np.eye(self.nv))) elif firstsecond == crocoddyl.Jcomponent.second: dx = self.diff(x1, x2) q = x1[:self.nq] dq = dx[:self.nv] Jdq = pinocchio.dIntegrate(self.model, q, dq)[1] return np.matrix(scl.block_diag(np.linalg.inv(Jdq), np.eye(self.nv)))
def Jdiff(self, x1, x2, firstsecond='both'): assert (firstsecond in ['first', 'second', 'both']) if firstsecond == 'both': return [self.Jdiff(x1, x2, 'first'), self.Jdiff(x1, x2, 'second')] if firstsecond == 'first': dx = self.diff(x2, x1) q = x2[:self.model.nq] dq = dx[:self.model.nv] Jdq = pinocchio.dIntegrate(self.model, q, dq)[1] return np.matrix(-scl.block_diag(np.linalg.inv(Jdq), np.eye(self.nv))) elif firstsecond == 'second': dx = self.diff(x1, x2) q = x1[:self.nq] dq = dx[:self.nv] Jdq = pinocchio.dIntegrate(self.model, q, dq)[1] return np.matrix(scl.block_diag(np.linalg.inv(Jdq), np.eye(self.nv)))
def dCostQ_dx(self, x): ''' ddiff_dx2(x1,x2) = dint_dv(x1,x2-x1)^-1 ddiff_dq( refQ,q) = dint_dv(refQ,q-refQ) ''' q = self.vq2q(a2m(x)) dq = pinocchio.difference(self.rmodel, self.refQ, q) dDiff = inv(pinocchio.dIntegrate(self.rmodel, self.refQ, dq)[1]) grad = dDiff[6:, :].T * dq[6:] return m2a(grad)
def Jintegrate(self, x, dx, firstsecond='both'): assert (firstsecond in ['first', 'second', 'both']) if firstsecond == 'both': return [self.Jintegrate(x, dx, 'first'), self.Jintegrate(x, dx, 'second')] q = x[:self.nq] dq = dx[:self.nv] Jq, Jdq = pinocchio.dIntegrate(self.model, q, dq) if firstsecond == 'first': return np.matrix(scl.block_diag(np.linalg.inv(Jq), np.eye(self.nv))) elif firstsecond == 'second': return np.matrix(scl.block_diag(np.linalg.inv(Jdq), np.eye(self.nv)))
def test_derivatives(self): model = self.model q = self.q v = self.v J0, J1 = pin.dIntegrate(model, q, v) res_0 = pin.dIntegrate(model, q, v, pin.ARG0) res_1 = pin.dIntegrate(model, q, v, pin.ARG1) self.assertApprox(J0, res_0) self.assertApprox(J1, res_1) q_next = pin.integrate(model, q, v) J0, J1 = pin.dDifference(model, q, q_next) res_0 = pin.dDifference(model, q, q_next, pin.ARG0) res_1 = pin.dDifference(model, q, q_next, pin.ARG1) self.assertApprox(J0, res_0) self.assertApprox(J1, res_1)
def test_transport(self): model = self.model Jq, Jv = pin.dIntegrate(model, self.q, self.v) mat = np.random.randn(model.nv, 2) mat_transported_q = pin.dIntegrateTransport(model, self.q, self.v, mat, pin.ARG0) mat_transported_v = pin.dIntegrateTransport(model, self.q, self.v, mat, pin.ARG1) self.assertApprox(mat_transported_q, np.dot(Jq, mat)) self.assertApprox(mat_transported_v, np.dot(Jv, mat))
def Jintegrate(self, x, dx, firstsecond=crocoddyl.Jcomponent.both): if firstsecond == crocoddyl.Jcomponent.both: return [ self.Jintegrate(x, dx, crocoddyl.Jcomponent.first), self.Jintegrate(x, dx, crocoddyl.Jcomponent.second) ] q = x[:self.nq] dq = dx[:self.nv] Jq, Jdq = pinocchio.dIntegrate(self.model, q, dq) if firstsecond == crocoddyl.Jcomponent.first: return np.matrix(scl.block_diag(np.linalg.inv(Jq), np.eye(self.nv))) elif firstsecond == crocoddyl.Jcomponent.second: return np.matrix(scl.block_diag(np.linalg.inv(Jdq), np.eye(self.nv)))
def Jintegrate(self, x, dx, firstsecond='both'): """ Compute the partial derivatives for integrate operator using Pinocchio. :param x: current state :param dx: displacement of the state :param firstsecond: desired partial derivative :return the partial derivative(s) of the integrate(x,dx) function """ assert (firstsecond in ['first', 'second', 'both']) assert (x.shape == (self.nx, ) and dx.shape == (self.ndx, )) if firstsecond == 'both': return [self.Jintegrate(x, dx, 'first'), self.Jintegrate(x, dx, 'second')] q = a2m(x[:self.model.nq]) dq = a2m(dx[:self.model.nv]) Jq, Jdq = pinocchio.dIntegrate(self.model, q, dq) if firstsecond == 'first': # Derivative wrt x return block_diag(np.asarray(Jq), np.eye(self.model.nv)) else: # Derivative wrt v return block_diag(np.asarray(Jdq), np.eye(self.model.nv))
def dq_dv(self, vq): return pinocchio.dIntegrate(self.rmodel, self.q0, vq)[1]