def calc(self, data, x, u=None):
if u is None:
u = self.unone
nq, nv = self.nq, self.nv
q = a2m(x[:nq])
v = a2m(x[-nv:])
pinocchio.computeAllTerms(self.pinocchio, data.pinocchio, q, v)
pinocchio.updateFramePlacements(self.pinocchio, data.pinocchio)
data.tauq[:] = self.actuation.calc(data.actuation, x, u)
self.contact.calc(data.contact, x)
data.K[:nv, :nv] = data.pinocchio.M
if hasattr(self.pinocchio, 'armature'):
data.K[range(nv), range(nv)] += self.pinocchio.armature.flat
data.K[nv:, :nv] = data.contact.J
data.K.T[nv:, :nv] = data.contact.J
data.Kinv = np.linalg.inv(data.K)
data.r[:nv] = data.tauq - m2a(data.pinocchio.nle)
data.r[nv:] = -data.contact.a0
data.af[:] = np.dot(data.Kinv, data.r)
data.f[:] *= -1.
# Convert force array to vector of spatial forces.
self.contact.setForces(data.contact, data.f)
data.cost = self.costs.calc(data.costs, x, u)
return data.xout, data.cost
def calcDiff(self, data, x, u, recalc=True):
if recalc:
self.calc(data, x, u)
# data.Ru[:,:] = np.eye(nu)
Ax, Axx = self.activation.calcDiff(data.activation,
data.residuals,
recalc=recalc)
data.Lu[:] = Ax
data.Luu[:, :] = np.diag(m2a(Axx))
def calc(self, data, x):
# We suppose forwardKinematics(q,v,np.zero), computeJointJacobian and updateFramePlacement already
# computed.
assert (self.ref is not None or self.gains[0] == 0.)
data.v = pinocchio.getFrameVelocity(self.pinocchio, data.pinocchio,
self.frame).copy()
vw = data.v.angular
vv = data.v.linear
J6 = pinocchio.getFrameJacobian(self.pinocchio, data.pinocchio,
self.frame,
pinocchio.ReferenceFrame.LOCAL)
data.J[:, :] = J6[:3, :]
data.Jw[:, :] = J6[3:, :]
data.a0[:] = (pinocchio.getFrameAcceleration(
self.pinocchio, data.pinocchio, self.frame).linear +
cross(vw, vv)).flat
if self.gains[0] != 0.:
data.a0[:] += self.gains[0] * (
m2a(data.pinocchio.oMf[self.frame].translation) - self.ref)
if self.gains[1] != 0.:
data.a0[:] += self.gains[1] * m2a(vv)
def calc(self, data, x, u):
data.residuals = m2a(data.pinocchio.com[0]) - self.ref
data.cost = sum(self.activation.calc(data.activation, data.residuals))
return data.cost
def calc(self, data, x, u):
data.rRf = self.ref.transpose() * data.pinocchio.oMf[
self.frame].rotation
data.residuals[:] = m2a(pinocchio.log3(data.rRf))
data.cost = sum(self.activation.calc(data.activation, data.residuals))
return data.cost
def calc(self, data, x, u):
data.rMf = self.ref.inverse() * data.pinocchio.oMf[self.frame]
data.residuals[:] = m2a(pinocchio.log(data.rMf).vector)
data.cost = sum(self.activation.calc(data.activation, data.residuals))
return data.cost
def calc(self, data, x, u):
data.residuals[:] = m2a(
pinocchio.getFrameVelocity(self.pinocchio, data.pinocchio,
self.frame).linear) - self.ref
data.cost = sum(self.activation.calc(data.activation, data.residuals))
return data.cost
def calc(self, data, x, u):
data.residuals = m2a(
data.pinocchio.oMf[self.frame].translation) - self.ref
data.cost = sum(self.activation.calc(data.activation, data.residuals))
return data.cost