def __init__(self, model, pinocchioData):
nc, nv = model.nimpulse, model.nv
self.pinocchio = pinocchioData
self.J = np.zeros([nc, nv])
self.Jq = np.zeros([nc, nv])
self.f = np.nan # not set at construction type
self.forces = pinocchio.StdVect_Force()
for i in range(model.pinocchio.njoints):
self.forces.append(pinocchio.Force.Zero())
self.Vq = np.zeros([nc, nv])
def __init__(self, model, pinocchioData):
nc, nv, ndx = model.ncontact, model.nv, model.ndx
self.pinocchio = pinocchioData
self.J = np.zeros([nc, nv])
self.a0 = np.zeros(nc)
self.Ax = np.zeros([nc, ndx])
self.Aq = self.Ax[:, :nv]
self.Av = self.Ax[:, nv:]
self.f = np.nan # not set at construction type
self.forces = pinocchio.StdVect_Force()
for i in range(model.pinocchio.njoints):
self.forces.append(pinocchio.Force.Zero())
def constraint_dyn(self, x, nc=0):
'''
M aq + b(q,vq) + g(q) = tau_q + J(q)^T f
= rnea(q,vq=0,aq=0,fs)
'''
q, tauq, fr, fl = self.x2qf(x)
# Forces should be stored in RNEA-compatible structure
forces = pinocchio.StdVect_Force()
for i in range(self.rmodel.njoints):
forces.append(pinocchio.Force.Zero())
# Forces should be expressed at the joint for RNEA, while we store them
# at the frame in the optim problem. Convert.
jMr = rmodel.frames[self.idR].placement
forces[self.jidR] = jMr * pinocchio.Force(fr)
jMl = rmodel.frames[self.idL].placement
forces[self.jidL] = jMl * pinocchio.Force(fl)
#q = self.rmodel.neutralConfiguration
aq = vq = zero(self.rmodel.nv)
tauref = pinocchio.rnea(self.rmodel, self.rdata, q, vq, aq, forces)
self.eq[nc:nc + self.rmodel.nv] = (tauref - tauq).flat
return self.eq[nc:nc + self.rmodel.nv].tolist()
h) # threshold was 1e-3, is now 2.11e-4 (see assertNumDiff.__doc__)
# Check RNEA versus ABA derivatives.
Mi = pinocchio.computeMinverse(model, data, q)
assert (absmax(Mi - inv(data.M)) < 1e-6)
D = np.dot(Mi, data.dtau_dq)
assertNumDiff(
D, -data.ddq_dq, NUMDIFF_MODIFIER *
h) # threshold was 1e-3, is now 2.11e-4 (see assertNumDiff.__doc__)
# ---- ABA AND RNEA with forces
# Set forces container
fs = pinocchio.StdVect_Force()
for i in range(model.njoints):
fs.append(pinocchio.Force.Random())
# Check RNEA derivatives versus finite-diff (with forces)
a = pinocchio.aba(model, data, q, v, tau, fs)
dtau_dqn = df_dq(model, lambda q_: pinocchio.rnea(model, data, q_, v, a, fs),
q)
pinocchio.computeRNEADerivatives(model, data, q, v, a, fs)
dtau_dq = data.dtau_dq.copy()
assertNumDiff(
dtau_dqn, dtau_dq, NUMDIFF_MODIFIER *
h) # threshold was 1e-3, is now 2.11e-4 (see assertNumDiff.__doc__)
# Check ABA derivatives versus finite diff (with forces)
da_dqn = df_dq(model, lambda q_: pinocchio.aba(model, data, q_, v, tau, fs), q)
def ForceVect(force_list):
'''Convert a list of forces into a StdVect_Force'''
res = se3.StdVect_Force()
res.extend(f for f in force_list)
return res
def ForceDict(force_dict, N):
'''Convert a dict of forces into a StdVect_Force'''
res = se3.StdVect_Force()
res.extend(se3.Force.Zero() if i not in force_dict else force_dict[i]
for i in range(N))
return res