def calcDiff(self, data, x, u=None, recalc=True):
if recalc:
self.calc(data, x, u)
self.assertImpactDiffDataSet(data)
nv = self.pinocchio.nv
Ax, Axx = self.activation.calcDiff(data.activation, data.residuals)
# TODO ???
# r = Jcom(vnext-v)
# dr/dv = Jcom*(dvnext/dv - I)
# dr/dq = dJcom_dq*(vnext-v) + Jcom*dvnext_dq
# = dvcom_dq(vq=vnext-v) + Jcom*dvnext_dq
# Jcom*v = M[:3,:]/mass * v = RNEA(q,0,v)[:3]/mass
# => dvcom/dq = dRNEA_dq(q,0,v)[:3,:]/mass
dvc_dq = pinocchio.getCenterOfMassVelocityDerivatives(
self.pinocchio, data.pinocchio_dv)
dvc_dv = pinocchio.jacobianCenterOfMass(self.pinocchio,
data.pinocchio_dv)
# res = vcom(q,vnext-v)
# dres/dq = dvcom_dq + dvcom_dv*dvnext_dq
data.Rx[:, :nv] = dvc_dq + np.dot(dvc_dv, data.dvnext_dx[:, :nv])
# dres/dv = dvcom_dv*(dvnext_dv-I)
ddv_dv = data.dvnext_dx[:, nv:].copy()
ddv_dv[range(nv), range(nv)] -= 1
data.Rx[:, nv:] = np.dot(dvc_dv, ddv_dv)
data.Lx[:] = np.dot(data.Rx.T, Ax)
data.Lxx[:, :] = np.dot(data.Rx.T, Axx * data.Rx)
def test_numdiff(self):
rmodel,rdata = self.rmodel,self.rdata
q,vq,aq,dq= self.q,self.vq,self.aq,self.dq
#### Compute d/dq VCOM with the algo.
pin.computeAllTerms(rmodel,rdata,q,vq)
pin.computeForwardKinematicsDerivatives(rmodel,rdata,q,vq,aq)
dvc_dq = pin.getCenterOfMassVelocityDerivatives(rmodel,rdata)
#### Approximate d/dq VCOM by finite diff.
def calc_vc(q,vq):
""" Compute COM velocity """
pin.centerOfMass(rmodel,rdata,q,vq)
return rdata.vcom[0]
dvc_dqn = df_dq(rmodel,lambda _q: calc_vc(_q,vq),q)
self.assertTrue(np.allclose(dvc_dq,dvc_dqn,atol=np.sqrt(self.precision)))
def test_numdiff(self):
rmodel, rdata = self.rmodel, self.rdata
rdata_fd = self.rdata_fd
q, vq = self.q, self.vq
#### Compute d/dq VCOM with the algo.
pin.computeAllTerms(rmodel, rdata, q, vq)
dvc_dq = pin.getCenterOfMassVelocityDerivatives(rmodel, rdata)
#### Approximate d/dq VCOM by finite diff.
def calc_vc(q, vq):
""" Compute COM velocity """
pin.centerOfMass(rmodel, rdata_fd, q, vq)
return rdata_fd.vcom[0].copy()
dvc_dqn = df_dq(rmodel, lambda _q: calc_vc(_q, vq), q)
self.assertTrue(
np.allclose(dvc_dq, dvc_dqn, atol=np.sqrt(self.precision)))