'''
(a,f) = K^-1 (tau-b,-gamma)
avec K = [ M J* ; J 0 ]
(a',f') = -K^-1 K' K^-1 (tau-b,-gamma) - K^-1 (b';gamma')
= -Ki K' (a,f) - Ki (b';g')
= -Ki ( K'(a,f) - (b',g') )
'''
# Define finite-diff routines.
# Check ABA derivatives (without forces)
da_dq = df_dq(model, lambda q_: pinocchio.aba(model, data, q_, v, tau), q)
da_dv = df_dx(lambda v_: pinocchio.aba(model, data, q, v_, tau), v)
pinocchio.computeABADerivatives(model, data, q, v, tau)
h = np.sqrt(2 * EPS)
assertNumDiff(
da_dq, data.ddq_dq, NUMDIFF_MODIFIER *
h) # threshold was 1e-2, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(
da_dv, data.ddq_dv, NUMDIFF_MODIFIER *
h) # threshold was 1e-2, is now 2.11e-4 (see assertNumDiff.__doc__)
# Check RNEA Derivatives (without forces)
a = pinocchio.aba(model, data, q, v, tau)
dtau_dq = df_dq(model, lambda q_: pinocchio.rnea(model, data, q_, v, a), q)
pinocchio.computeRNEADerivatives(model, data, q, v, a)
contactData2 = contactModel.createData(rdata2)
contactModel.calc(contactData2, x)
assert (norm(contactData.a0 - contactData2.a0) < 1e-9)
assert (norm(contactData.J - contactData2.J) < 1e-9)
def returna_at0(q, v):
x = np.vstack([q, v]).flat
pinocchio.computeAllTerms(rmodel, rdata2, q, v)
pinocchio.updateFramePlacements(rmodel, rdata2)
contactModel.calc(contactData2, x)
return a2m(contactData2.a0) # .copy()
Aq_numdiff = df_dq(rmodel, lambda _q: returna_at0(_q, v), q)
Av_numdiff = df_dx(lambda _v: returna_at0(q, _v), v)
assertNumDiff(contactData.Aq, Aq_numdiff, NUMDIFF_MODIFIER * np.sqrt(
2 * EPS)) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(contactData.Av, Av_numdiff, NUMDIFF_MODIFIER * np.sqrt(
2 * EPS)) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__)
eps = 1e-8
Aq_numdiff = df_dq(rmodel, lambda _q: returna_at0(_q, v), q, h=eps)
Av_numdiff = df_dx(lambda _v: returna_at0(q, _v), v, h=eps)
assert (np.isclose(contactData.Aq, Aq_numdiff, atol=np.sqrt(eps)).all())
assert (np.isclose(contactData.Av, Av_numdiff, atol=np.sqrt(eps)).all())
contactModel = ContactModel3D(
rmodel,
def dy_dv(i):
return df_dx(lambda _v: get_y(a2m(x[:nq]), _v)[i], a2m(x[nq:]))
def dy_dq(i):
return df_dx(lambda _q: get_y(_q, a2m(x[nq:]))[i], a2m(x[:nq]))
def dk_du(i):
return df_dx(lambda _u: get_ku(_u)[i], a2m(u))
model.calc(data, x, m2a(u))
return a2m(data.xnext) # .copy()
def get_au(u):
a, _ = model.differential.calc(data.differential[0], x, m2a(u))
return a2m(a)
def get_y(q, v):
x_ = np.vstack([q, v])
model.calc(data, m2a(x_), u)
return [a2m(y) for y in data.y]
dxn_du = df_dx(lambda _u: get_xn(_u), a2m(u))
def dk_du(i):
return df_dx(lambda _u: get_ku(_u)[i], a2m(u))
def dk_dq(i):
return df_dx(lambda _q: get_k(_q, a2m(x[nq:]))[i], a2m(x[:nq]))
def dk_dv(i):
return df_dx(lambda _v: get_k(a2m(x[:nq]), _v)[i], a2m(x[nq:]))
def dy_dq(i):