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