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 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=None, recalc=True):
if u is None:
u = self.unone
if recalc:
xout, cost = self.calc(data, x, u)
nq, nv = self.nq, self.nv
q = a2m(x[:nq])
v = a2m(x[-nv:])
tauq = a2m(u)
a = a2m(data.xout)
# --- Dynamics
if self.forceAba:
pinocchio.computeABADerivatives(self.pinocchio, data.pinocchio, q,
v, tauq)
data.Fx[:, :nv] = data.pinocchio.ddq_dq
data.Fx[:, nv:] = data.pinocchio.ddq_dv
data.Fu[:, :] = data.pinocchio.Minv
else:
pinocchio.computeRNEADerivatives(self.pinocchio, data.pinocchio, q,
v, a)
data.Fx[:, :nv] = -np.dot(data.Minv, data.pinocchio.dtau_dq)
data.Fx[:, nv:] = -np.dot(data.Minv, data.pinocchio.dtau_dv)
data.Fu[:, :] = data.Minv
# --- Cost
pinocchio.computeJointJacobians(self.pinocchio, data.pinocchio, q)
pinocchio.updateFramePlacements(self.pinocchio, data.pinocchio)
self.costs.calcDiff(data.costs, x, u, recalc=False)
return data.xout, data.cost
def calcDiff(self, data, x, u=None, recalc=True):
if u is None:
u = self.unone
if recalc:
xout, cost = self.calc(data, x, u)
nq, nv = self.nq, self.nv
q = a2m(x[:nq])
v = a2m(x[-nv:])
a = a2m(data.a)
fs = data.contact.forces
pinocchio.computeRNEADerivatives(self.pinocchio, data.pinocchio, q, v, a, fs)
pinocchio.updateFramePlacements(self.pinocchio, data.pinocchio)
# [a;-f] = K^-1 [ tau - b, -gamma ]
# [a';-f'] = -K^-1 [ K'a + b' ; J'a + gamma' ] = -K^-1 [ rnea'(q,v,a,fs) ; acc'(q,v,a) ]
# Derivative of the actuation model tau = tau(q,u)
# dtau_dq and dtau_dv are the rnea derivatives rnea'
did_dq = data.pinocchio.dtau_dq
did_dv = data.pinocchio.dtau_dv
# Derivative of the contact constraint
# da0_dq and da0_dv are the acceleration derivatives acc'
self.contact.calcDiff(data.contact, x, recalc=False)
dacc_dq = data.contact.Aq
dacc_dv = data.contact.Av
data.Kinv = np.linalg.inv(data.K)
# We separate the Kinv into the a and f rows, and the actuation and acceleration columns
da_did = -data.Kinv[:nv, :nv]
df_did = data.Kinv[nv:, :nv]
da_dacc = -data.Kinv[:nv, nv:]
df_dacc = data.Kinv[nv:, nv:]
da_dq = np.dot(da_did, did_dq) + np.dot(da_dacc, dacc_dq)
da_dv = np.dot(da_did, did_dv) + np.dot(da_dacc, dacc_dv)
da_dtau = data.Kinv[:nv, :nv] # Add this alias just to make the code clearer
df_dtau = -data.Kinv[nv:, :nv] # Add this alias just to make the code clearer
# tau is a function of x and u (typically trivial in x), whose derivatives are Ax and Au
dtau_dx = data.actuation.Ax
dtau_du = data.actuation.Au
data.Fx[:, :nv] = da_dq
data.Fx[:, nv:] = da_dv
data.Fx += np.dot(da_dtau, dtau_dx)
data.Fu[:, :] = np.dot(da_dtau, dtau_du)
data.df_dq[:, :] = np.dot(df_did, did_dq) + np.dot(df_dacc, dacc_dq)
data.df_dv[:, :] = np.dot(df_did, did_dv) + np.dot(df_dacc, dacc_dv)
data.df_dx += np.dot(df_dtau, dtau_dx)
data.df_du[:, :] = np.dot(df_dtau, dtau_du)
self.contact.setForcesDiff(data.contact, data.df_dx, data.df_du)
self.costs.calcDiff(data.costs, x, u, recalc=False)
return data.xout, data.cost
def integrate(self, x, dx):
""" Integrate the current robot's state.
:param x: current state
:param dx: displacement of the state
:return x [+] dx value
"""
nq, nv, nx, ndx = self.model.nq, self.model.nv, self.nx, self.ndx
assert (x.shape == (nx, ) and dx.shape == (ndx, ))
q = x[:nq]
v = x[-nv:]
dq = dx[:nv]
dv = dx[-nv:]
qn = pinocchio.integrate(self.model, a2m(q), a2m(dq))
return np.concatenate([qn.flat, v + dv])
def diff(self, x0, x1):
""" Difference between the robot's states x2 and x1.
:param x1: current state
:param x2: next state
:return x2 [-] x1 value
"""
nq, nv, nx = self.model.nq, self.model.nv, self.nx
assert (x0.shape == (nx, ) and x1.shape == (nx, ))
q0 = x0[:nq]
q1 = x1[:nq]
v0 = x0[-nv:]
v1 = x1[-nv:]
dq = pinocchio.difference(self.model, a2m(q0), a2m(q1))
return np.concatenate([dq.flat, v1 - v0])
def setForces(self, data, forcesArr, forcesVec=None):
'''
Convert a numpy array of forces into a stdVector of spatial forces.
'''
# In the dynamic equation, we wrote M*a + J.T*fdyn, while in the ABA it would be
# M*a + b = tau + J.T faba, so faba = -fdyn (note the minus operator before a2m).
data.f = forcesArr
if forcesVec is None:
forcesVec = data.forces
data.forces[data.joint] *= 0
forcesVec[data.joint] += data.jMf * pinocchio.Force(
a2m(forcesArr), np.zeros((3, 1)))
return forcesVec
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:])
tauq = a2m(u)
# --- Dynamics
if self.forceAba:
data.xout[:] = pinocchio.aba(self.pinocchio, data.pinocchio, q, v,
tauq).flat
else:
pinocchio.computeAllTerms(self.pinocchio, data.pinocchio, q, v)
data.M = data.pinocchio.M
if hasattr(self.pinocchio, 'armature'):
data.M[range(nv), range(nv)] += self.pinocchio.armature.flat
data.Minv = np.linalg.inv(data.M)
data.xout[:] = data.Minv * (tauq - data.pinocchio.nle).flat
# --- Cost
pinocchio.forwardKinematics(self.pinocchio, data.pinocchio, q, v)
pinocchio.updateFramePlacements(self.pinocchio, data.pinocchio)
data.cost = self.costs.calc(data.costs, x, u)
return data.xout, data.cost
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))