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(data.actuation.a)
pinocchio.computeABADerivatives(self.pinocchio, data.pinocchio, q, v, tauq)
da_dq = data.pinocchio.ddq_dq
da_dv = data.pinocchio.ddq_dv
da_dact = data.pinocchio.Minv
dact_dx = data.actuation.Ax
dact_du = data.actuation.Au
data.Fx[:, :nv] = da_dq
data.Fx[:, nv:] = da_dv
data.Fx += np.dot(da_dact, dact_dx)
data.Fu[:, :] = np.dot(da_dact, dact_du)
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):
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):
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 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 f(self, x, u, t, jacobian=False):
nq = self.robot.nq
nv = self.robot.nv
model = self.robot.model
data = self.robot.data
q = x[:nq]
v = x[nq:]
if (nv == 1):
# for 1 DoF systems pin.aba does not work (I don't know why)
pin.computeAllTerms(model, data, q, v)
ddq = (u - data.nle) / data.M[0]
else:
ddq = pin.aba(model, data, q, v, u)
self.dx[:nv] = v
self.dx[nv:] = ddq
if (jacobian):
pin.computeABADerivatives(model, data, q, v, u)
self.Fx[:nv, :nv] = 0.0
self.Fx[:nv, nv:] = np.identity(nv)
self.Fx[nv:, :nv] = data.ddq_dq
self.Fx[nv:, nv:] = data.ddq_dv
self.Fu[nv:, :] = data.Minv
return (np.copy(self.dx), np.copy(self.Fx), np.copy(self.Fu))
return np.copy(self.dx)
def test_aba_derivatives(self):
res = pin.computeABADerivatives(self.model,self.data,self.q,self.v,self.tau)
self.assertTrue(len(res) == 3)
data2 = self.model.createData()
pin.aba(self.model,data2,self.q,self.v,self.tau)
self.assertApprox(self.data.ddq,data2.ddq)
# With external forces
res = pin.computeABADerivatives(self.model,self.data,self.q,self.v,self.tau,self.fext)
self.assertTrue(len(res) == 3)
pin.aba(self.model,data2,self.q,self.v,self.tau,self.fext)
self.assertApprox(self.data.ddq,data2.ddq)
def jacobian(x, u):
jacx = np.zeros((12, 12))
jacu = np.zeros((12, 6))
q = x[:6]
dq = x[6:]
a = pinocchio.computeABADerivatives(robot.model, robot.data, q, dq, u)
jacx[:6, 6:] = np.eye(6)
jacx[6:, :6] = robot.data.ddq_dq
jacx[6:, 6:] = robot.data.ddq_dv
jacu[6:, :] = robot.data.Minv
return jacx, jacu
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 ## forward dynamics, using the algorithms explained 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 tau = np.matrix(np.random.rand(model.nv, 1)) # joint acceleration # Evaluate the derivatives pin.computeABADerivatives(model, data, q, v, tau) # Retrieve the derivatives in data ddq_dq = data.ddq_dq # Derivatives of the FD w.r.t. the joint config vector ddq_dv = data.ddq_dv # Derivatives of the FD w.r.t. the joint velocity vector ddq_dtau = data.Minv # Derivatives of the FD 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
for k in range(m.njoints):
fext.append(pin.Force.Zero())
# Evaluate all terms and forward dynamics
pin.computeAllTerms(m, d, q, v)
a = pin.aba(m, d, q, v, u, fext)
# Print state and controls
print("pos =", q)
print("vel =", v)
print("tau =", u)
print("acc =", a)
print("nle =", d.nle)
# Evaluate the ABA derivatives
pin.computeABADerivatives(m, d, q, v, u, fext)
print("\nABA derivatives:")
print("ddq_dq:\n", d.ddq_dq)
print("ddq_dv:\n", d.ddq_dv)
print("M:\n", d.M)
print("Minv:\n", d.Minv)
# Evaluate the sensitivities of M & Minv to configuration perturbations
M = d.M
Minv = d.Minv
dM_dq = np.zeros((m.nv, m.nv))
eps = 1e-6
for i in range(m.nv):
q[i] += eps
print("pos =", q)
pin.computeAllTerms(m, d, q, v)
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
def f(self, x, u, t, jacobian=False):
nq = self.robot.nq
nv = self.robot.nv
model = self.robot.model
data = self.robot.data
q = x[:nq]
v = x[nq:]
i = 0
self.robot.computeAllTerms(q, v)
for cs in self.contact_surfaces: # for each candidate contact surface
for cp in self.contact_points: # for each candidate contact point
# Contact point placement
H = self.robot.framePlacement(q, cp.frame_id, False)
p_c = H.translation
if (cs.check_collision(p_c)
): # check whether the point is colliding with the surface
if (cp.active == False
): # if the contact was not already active
cp.active = True
cp.p0 = np.copy(p_c) # anchor point
# Compute the contact force
self.fc[i:i + 3] = cs.compute_force(cp, q, v, self.robot)
# compute the jacobian
self.Jc[i:i + 3, :] = cp.get_jacobian()
i += 3
else: # if the point is not colliding more
if (cp.active): # if the contact was already active
cp.active = False
# Contact force equal to 0
self.fc[i:i + 3] = np.zeros(3)
# jacobian equl to zero
self.Jc[i:i + 3, :] = np.zeros((3, self.robot.model.nv))
i += 3
# compute JT*force from contact point
u_con = u + self.Jc.T.dot(self.fc)
if (nv == 1):
# for 1 DoF systems pin.aba does not work (I don't know why)
ddq = (u_con - data.nle) / data.M[0]
else:
ddq = pin.aba(model, data, q, v, u_con)
self.dx[:nv] = v
self.dx[nv:] = ddq
if (jacobian):
pin.computeABADerivatives(model, data, q, v, u_con)
self.Fx[:nv, :nv] = 0.0
self.Fx[:nv, nv:] = np.identity(nv)
self.Fx[nv:, :nv] = data.ddq_dq
self.Fx[nv:, nv:] = data.ddq_dv
self.Fu[nv:, :] = data.Minv
return (np.copy(self.dx), np.copy(self.Fx), np.copy(self.Fu))
return np.copy(self.dx)
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
## ## In this short script, we show how to compute the derivatives of the ## forward dynamics, using the algorithms explained 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 tau = np.matrix(np.random.rand(model.nv,1)) # joint acceleration # Evaluate the derivatives pin.computeABADerivatives(model,data,q,v,tau) # Retrieve the derivatives in data ddq_dq = data.ddq_dq # Derivatives of the FD w.r.t. the joint config vector ddq_dv = data.ddq_dv # Derivatives of the FD w.r.t. the joint velocity vector ddq_dtau = data.Minv # Derivatives of the FD w.r.t. the joint acceleration vector
