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]