def calcDiff(self, data, x, u=None):
self.costsData.shareMemory(data)
nq, nv = self.state.nq, self.state.nv
q, v = x[:nq], x[-nv:]
if u is None:
u = self.unone
if True:
self.calc(data, x, u)
self.actuation.calcDiff(self.actuationData, x, u)
tau = self.actuationData.tau
# Computing the dynamics derivatives
if self.enable_force:
pinocchio.computeABADerivatives(self.state.pinocchio,
self.pinocchioData, q, v, tau)
ddq_dq = self.pinocchioData.ddq_dq
ddq_dv = self.pinocchioData.ddq_dv
data.Fx[:, :] = np.hstack([
ddq_dq, ddq_dv
]) + self.pinocchioData.Minv * self.actuationData.dtau_dx
data.Fu[:, :] = self.pinocchioData.Minv * self.actuationData.dtau_du
else:
pinocchio.computeRNEADerivatives(self.state.pinocchio,
self.pinocchioData, q, v,
data.xout)
ddq_dq = self.Minv * (self.actuationData.dtau_dx[:, :nv] -
self.pinocchioData.dtau_dq)
ddq_dv = self.Minv * (self.actuationData.dtau_dx[:, nv:] -
self.pinocchioData.dtau_dv)
data.Fx[:, :] = np.hstack([ddq_dq, ddq_dv])
data.Fu[:, :] = self.Minv * self.actuationData.dtau_du
# Computing the cost derivatives
self.costs.calcDiff(self.costsData, x, u)
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):
self.costsData.shareMemory(data)
nq, nv = self.state.nv, self.state.nq
q, v = x[:nq], x[-nv:]
self.actuation.calcDiff(self.actuationData, x, u)
if u is None:
u = self.unone
if recalc:
self.calc(data, x, u)
pinocchio.computeJointJacobians(self.state.pinocchio, self.pinocchioData, q)
# Computing the dynamics derivatives
if self.enable_force:
pinocchio.computeABADerivatives(self.state.pinocchio, self.pinocchioData, q, v, u)
ddq_dq = self.pinocchioData.ddq_dq
ddq_dv = self.pinocchioData.ddq_dv
data.Fx = np.hstack([ddq_dq, ddq_dv]) + self.pinocchioData.Minv * self.actuationData.dtau_dx
data.Fu = self.pinocchioData.Minv * self.actuationData.dtau_du
else:
pinocchio.computeRNEADerivatives(self.state.pinocchio, self.pinocchioData, q, v, data.xout)
ddq_dq = self.Minv * (self.actuationData.dtau_dx[:, :nv] - self.pinocchioData.dtau_dq)
ddq_dv = self.Minv * (self.actuationData.dtau_dx[:, nv:] - self.pinocchioData.dtau_dv)
data.Fx = np.hstack([ddq_dq, ddq_dv])
data.Fu = self.Minv * self.actuationData.dtau_du
# Computing the cost derivatives
self.costs.calcDiff(self.costsData, x, u, False)
def main():
'''
Main procedure
1 ) Build the model from an URDF file
2 ) compute forwardKinematics
3 ) compute forwardKinematics of the frames
4 ) explore data
'''
model_file = Path(__file__).absolute().parents[1].joinpath(
'urdf', 'twolinks.urdf')
model = buildModelFromUrdf(str(model_file))
# Create data required by the algorithms
data = model.createData()
# Sample a random configuration
numpy_vector_joint_pos = randomConfiguration(model)
numpy_vector_joint_vel = np.random.rand(model.njoints - 1)
numpy_vector_joint_acc = np.random.rand(model.njoints - 1)
# Calls Reverese Newton-Euler algorithm
numpy_vector_joint_torques = rnea(model, data, numpy_vector_joint_pos,
numpy_vector_joint_vel,
numpy_vector_joint_acc)
computeRNEADerivatives(model, data, numpy_vector_joint_pos,
numpy_vector_joint_vel, numpy_vector_joint_acc)
dtorques_dq = data.dtau_dq
dtorques_dqd = data.dtau_dv
dtorques_dqdd = data.M
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 test_centroidal_derivatives(self):
res = pin.computeCentroidalDynamicsDerivatives(self.model,self.data,self.q,self.v,self.a)
self.assertTrue(len(res) == 4)
data2 = self.model.createData()
pin.computeCentroidalMomentumTimeVariation(self.model,data2,self.q,self.v,self.a)
self.assertApprox(self.data.hg,data2.hg)
self.assertApprox(self.data.dhg,data2.dhg)
data3 = self.model.createData()
pin.computeRNEADerivatives(self.model,data3,self.q,self.v,self.a)
res2 = pin.getCentroidalDynamicsDerivatives(self.model,data3)
for k in range(4):
self.assertApprox(res[k],res2[k])
def test_generalized_gravity_derivatives(self):
res = pin.computeGeneralizedGravityDerivatives(self.model, self.data,
self.q)
data2 = self.model.createData()
ref, _, _ = pin.computeRNEADerivatives(self.model, data2, self.q,
self.v * 0, self.a * 0)
self.assertApprox(res, ref)
def test_static_torque_derivatives(self):
res = pin.computeStaticTorqueDerivatives(self.model, self.data, self.q,
self.fext)
data2 = self.model.createData()
ref, _, _ = pin.computeRNEADerivatives(self.model, data2, self.q,
self.v * 0, self.a * 0,
self.fext)
self.assertApprox(res, ref)
def test_rnea_derivatives(self):
res = pin.computeRNEADerivatives(self.model, self.data, self.q, self.v,
self.a)
self.assertTrue(len(res) == 3)
data2 = self.model.createData()
pin.rnea(self.model, data2, self.q, self.v, self.a)
self.assertApprox(self.data.ddq, data2.ddq)
# With external forces
res = pin.computeRNEADerivatives(self.model, self.data, self.q, self.v,
self.a, self.fext)
self.assertTrue(len(res) == 3)
pin.rnea(self.model, data2, self.q, self.v, self.a, self.fext)
self.assertApprox(self.data.ddq, data2.ddq)
def calcDiff(self, data, x, u=None, recalc=True):
q, v = x[:self.state.nq], x[-self.state.nv:]
if u is None:
u = self.unone
if recalc:
self.calc(data, x, u)
pinocchio.computeJointJacobians(self.state.pinocchio,
data.pinocchio, q)
# Computing the dynamics derivatives
if self.forceAba:
pinocchio.computeABADerivatives(self.state.pinocchio,
data.pinocchio, q, v, u)
data.Fx = np.hstack([data.pinocchio.ddq_dq, data.pinocchio.ddq_dv])
data.Fu = data.pinocchio.Minv
else:
pinocchio.computeRNEADerivatives(self.state.pinocchio,
data.pinocchio, q, v, data.xout)
data.Fx = -np.hstack([
data.Minv * data.pinocchio.dtau_dq,
data.Minv * data.pinocchio.dtau_dv
])
data.Fu = data.Minv
# Computing the cost derivatives
self.costs.calcDiff(data.costs, x, u, False)
## ## In this short script, we show how to compute the derivatives of the ## inverse dynamics (RNEA), using the algorithms proposed in: ## ## Analytical Derivatives of Rigid Body Dynamics Algorithms, Justin Carpentier and Nicolas Mansard, Robotics: Science and Systems, 2018 ## # Create model and data model = pin.buildSampleModelHumanoidRandom() data = model.createData() # Set bounds (by default they are undefinded for a the Simple Humanoid model) model.lowerPositionLimit = -np.matrix(np.ones((model.nq, 1))) model.upperPositionLimit = np.matrix(np.ones((model.nq, 1))) q = pin.randomConfiguration(model) # joint configuration v = np.matrix(np.random.rand(model.nv, 1)) # joint velocity a = np.matrix(np.random.rand(model.nv, 1)) # joint acceleration # Evaluate the derivatives pin.computeRNEADerivatives(model, data, q, v, a) # Retrieve the derivatives in data dtau_dq = data.dtau_dq # Derivatives of the ID w.r.t. the joint config vector dtau_dv = data.dtau_dv # Derivatives of the ID w.r.t. the joint velocity vector dtau_da = data.M # Derivatives of the ID w.r.t. the joint acceleration vector
# if np.any(np.isinf(model.upperPositionLimit)):
# qmin = rmodel.lowerPositionLimit
# qmin[:7] = -1
# rmodel.lowerPositionLimit = qmin
# qmax = rmodel.upperPositionLimit
# qmax[:7] = 1
# rmodel.upperPositionLimit = qmax
q = pinocchio.randomConfiguration(model)
v = rand(model.nv) * 2 - 1
a = rand(model.nv) * 2 - 1
tau = pinocchio.rnea(model, data, q, v, a)
# Basic check of calling the derivatives.
pinocchio.computeABADerivatives(model, data, q, v, tau)
pinocchio.computeRNEADerivatives(model, data, q, v, a)
'''
a = M^-1 ( tau - b)
a' = -M^-1 M' M^-1 (tau-b) - M^-1 b'
= -M^-1 M' a - M^-1 b'
= -M^-1 (M'a + b')
= -M^-1 tau'
'''
# Check that a = J aq + Jdot vq
M = data.M
Mi = pinocchio.computeMinverse(model, data, q)
da = data.ddq_dq
dtau = data.dtau_dq
def calcDiff(self, data, x, u=None, recalc=True):
'''
k = [Mv;0]; K = [MJ^T;J0]
r = [vnext;f] = K^-1 k
dr/dv = K^-1 [M;0]
dr/dq = -K^-1 K'K^-1 k + K^-1 k' = -K^-1 (K'r-k')
= -K^-1 [ M'vnext + J'^T f- M'v ]
[ J'vnext ]
= -K^-1 [ M'(vnext-v) + J'^T f ]
[ J' vnext ]
'''
if recalc:
xout, cost = self.calc(data, x, u)
nq, nv = self.nq, self.nv
q = a2m(x[:nq])
v = a2m(x[-nv:])
vnext = a2m(data.vnext)
fs = data.impulse.forces
# Derivative M' dv + J'f + b'
g6bak = self.pinocchio.gravity.copy()
self.pinocchio.gravity = pinocchio.Motion.Zero()
pinocchio.computeRNEADerivatives(self.pinocchio, data.pinocchio, q,
zero(nv), vnext - v, fs)
self.pinocchio.gravity = g6bak
data.did_dq[:, :] = data.pinocchio.dtau_dq
# Derivative of the impulse constraint
pinocchio.computeForwardKinematicsDerivatives(self.pinocchio,
data.pinocchio, q, vnext,
zero(nv))
# pinocchio.updateFramePlacements(self.pinocchio,data.pinocchio)
self.impulse.calcDiff(data.impulse, x, recalc=False)
data.dv_dq = data.impulse.Vq
data.Fq[:nv, :] = 0
np.fill_diagonal(data.Fq[:nv, :], 1) # dq/dq
data.Fv[:nv, :] = 0 # dq/dv
data.Fx[nv:, :nv] = -np.dot(data.Kinv[:nv, :],
np.vstack([data.did_dq, data.dv_dq
])) # dvnext/dq
data.Fx[nv:, nv:] = np.dot(data.Kinv[:nv, :nv],
data.K[:nv, :nv]) # dvnext/dv
# data.Rx[:,:] = 0
# np.fill_diagonal(data.Rv,-1)
# data.Rx[:,:] += data.Fx[nv:,:]
# data.Rx *= self.impulseWeight
# data.Lx [:] = np.dot(data.Rx.T,data.costResiduals)
# data.Lxx[:,:] = np.dot(data.Rx.T,data.Rx)
if isinstance(self.costs, CostModelImpactBase):
self.costs.setImpactDiffData(data.costs, data.Fx[nv:, :])
if isinstance(self.costs, CostModelSum):
for cmodel, cdata in zip(self.costs.costs.values(),
data.costs.costs.values()):
if isinstance(cmodel.cost, CostModelImpactBase):
cmodel.cost.setImpactDiffData(cdata, data.Fx[nv:, :])
self.costs.calcDiff(data.costs, x, u=None, recalc=recalc)
return data.xnext, data.cost
def calcDiff(self, q):
pin.computeABADerivatives(robot.model, robot.data, q, self.v0, self.v0)
pin.computeRNEADerivatives(robot.model, robot.data, q, self.v0,
self.v0)
return -robot.data.dtau_dq.T @ robot.data.ddq - robot.data.ddq_dq.T @ robot.data.tau
def calcDiff(self, q):
pin.computeABADerivatives(self.rmodel, self.rdata, q, self.v0, self.v0)
pin.computeRNEADerivatives(self.rmodel, self.rdata, q, self.v0,
self.v0)
return -self.rdata.dtau_dq.T @ self.rdata.ddq - self.rdata.ddq_dq.T @ self.rdata.tau
# = -K^-1 [ M'(vnext-v) + J'^T f ]
# [ J' vnext ]
# Check M'(vnext-v)
# Check M(vnext-v)
def Mdv(q, vnext, v):
M = pinocchio.crba(rmodel, rdata, q)
return M * (vnext - v)
dn = df_dq(rmodel, lambda _q: Mdv(_q, a2m(data.vnext), v), q)
g = rmodel.gravity
rmodel.gravity = pinocchio.Motion.Zero()
pinocchio.computeRNEADerivatives(rmodel, rdata, q, zero(rmodel.nv),
a2m(data.vnext) - v)
d = rdata.dtau_dq.copy()
rmodel.gravity = g
assertNumDiff(
d, dn, NUMDIFF_MODIFIER * mnum.disturbance
) # threshold was 1e-5, is now 2.11e-4 (see assertNumDiff.__doc__)
# Check J.T f
np.set_printoptions(precision=4, linewidth=200, suppress=True)
def Jtf(q, f):
pinocchio.computeJointJacobians(rmodel, rdata, q)
pinocchio.forwardKinematics(rmodel, rdata, q)
pinocchio.updateFramePlacements(rmodel, rdata)
J = pinocchio.getFrameJacobian(rmodel, rdata, CONTACTFRAME,
## ## In this short script, we show how to compute the derivatives of the ## inverse dynamics (RNEA), using the algorithms proposed in: ## ## Analytical Derivatives of Rigid Body Dynamics Algorithms, Justin Carpentier and Nicolas Mansard, Robotics: Science and Systems, 2018 ## # Create model and data model = pin.buildSampleModelHumanoidRandom() data = model.createData() # Set bounds (by default they are undefinded for a the Simple Humanoid model) model.lowerPositionLimit = -np.matrix(np.ones((model.nq,1))) model.upperPositionLimit = np.matrix(np.ones((model.nq,1))) q = pin.randomConfiguration(model) # joint configuration v = np.matrix(np.random.rand(model.nv,1)) # joint velocity a = np.matrix(np.random.rand(model.nv,1)) # joint acceleration # Evaluate the derivatives pin.computeRNEADerivatives(model,data,q,v,a) # Retrieve the derivatives in data dtau_dq = data.dtau_dq # Derivatives of the ID w.r.t. the joint config vector dtau_dv = data.dtau_dv # Derivatives of the ID w.r.t. the joint velocity vector dtau_da = data.M # Derivatives of the ID w.r.t. the joint acceleration vector
