def test_Jdiff_and_Jintegrate_are_inverses(self):
# Generating random states
x1 = self.STATE.rand()
dx = np.random.rand(self.STATE.ndx)
x2 = self.STATE.integrate(x1, dx)
# Computing the partial derivatives of the integrate and difference function
Jx, Jdx = self.STATE.Jintegrate(x1, dx)
J1, J2 = self.STATE.Jdiff(x1, x2)
# Checking that Jdiff and Jintegrate are inverses
dX_dDX = Jdx
dDX_dX = J2
assertNumDiff(dX_dDX, np.linalg.inv(dDX_dX), 1e-9)
def test_Jdiff_against_numdiff(self):
# Generating random values for the initial and terminal states
x1 = self.STATE.rand()
x2 = self.STATE.rand()
# Computing the partial derivatives of the difference function
J1, J2 = self.STATE.Jdiff(x1, x2)
Jnum1, Jnum2 = self.STATE_NUMDIFF.Jdiff(x1, x2)
# Checking the partial derivatives against NumDiff
# The previous tolerance was 10*disturbance
tol = self.NUMDIFF_MOD * self.STATE_NUMDIFF.disturbance
assertNumDiff(J1, Jnum1, tol)
assertNumDiff(J2, Jnum2, tol)
def test_Jintegrate_against_numdiff(self):
# Generating random values for the initial state and its rate of change
x = self.STATE.rand()
vx = np.random.rand(self.STATE.ndx)
# Computing the partial derivatives of the integrate function
J1, J2 = self.STATE.Jintegrate(x, vx)
Jnum1, Jnum2 = self.STATE_NUMDIFF.Jintegrate(x, vx)
# Checking the partial derivatives against NumDiff
# The previous tolerance was 10*disturbance
tol = self.NUMDIFF_MOD * self.STATE_NUMDIFF.disturbance
assertNumDiff(J1, Jnum1, tol)
assertNumDiff(J2, Jnum2, tol)
x = model.State.rand()
# x[7:13] = 0
q = a2m(x[:model.nq])
v = a2m(x[model.nq:])
model.calc(data, x)
model.calcDiff(data, x)
mnum = ActionModelNumDiff(model, withGaussApprox=True)
dnum = mnum.createData()
nx, ndx, nq, nv, nu = model.nx, model.ndx, model.nq, model.nv, model.nu
mnum.calcDiff(dnum, x, None)
assertNumDiff(
dnum.Fx[:nv, :nv], data.Fx[:nv, :nv], NUMDIFF_MODIFIER * mnum.disturbance
) # threshold was 1e-3, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(
dnum.Fx[:nv, nv:], data.Fx[:nv, nv:], NUMDIFF_MODIFIER * mnum.disturbance
) # threshold was 1e-3, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(
dnum.Fx[nv:, nv:], data.Fx[nv:, nv:], NUMDIFF_MODIFIER * mnum.disturbance
) # threshold was 1e-3, is now 2.11e-4 (see assertNumDiff.__doc__)
rdata = rmodel.createData()
# = -K^-1 [ M'(vnext-v) + J'^T f ]
# [ J' vnext ]
# Check M'(vnext-v)
pinocchio.forwardKinematics(rmodel, rdata, q)
pinocchio.computeJointJacobians(rmodel, rdata, q)
rMi = Mref.inverse() * rdata.oMi[jid]
return np.dot(
pinocchio.Jlog6(rMi),
pinocchio.getJointJacobian(rmodel, rdata, jid,
pinocchio.ReferenceFrame.LOCAL))
def dresidualWorld(q):
pinocchio.forwardKinematics(rmodel, rdata, q)
pinocchio.computeJointJacobians(rmodel, rdata, q)
rMi = Mref.inverse() * rdata.oMi[jid]
return np.dot(
pinocchio.Jlog6(rMi),
pinocchio.getJointJacobian(rmodel, rdata, jid,
pinocchio.ReferenceFrame.WORLD))
d1 = dresidualLocal(q0)
d2 = dresidualWorld(q0)
d3 = df_dq(rmodel, residualrMi, q0)
pinocchio.forwardKinematics(rmodel, rdata, q0)
oMi = rdata.oMi[jid]
h = np.sqrt(2 * EPS)
assertNumDiff(
d1, d3, NUMDIFF_MODIFIER *
h) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__)
= -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)
assertNumDiff(
dtau_dq, data.dtau_dq, NUMDIFF_MODIFIER *
h) # threshold was 1e-3, is now 2.11e-4 (see assertNumDiff.__doc__)
# -----------------------------------------------------------------------------
robot = loadTalosArm(freeFloating=True)
qmin = robot.model.lowerPositionLimit
qmin[:7] = -1
robot.model.lowerPositionLimit = qmin
qmax = robot.model.upperPositionLimit
qmax[:7] = 1
robot.model.upperPositionLimit = qmax
rmodel = robot.model
rdata = rmodel.createData()
actModel = ActuationModelFreeFloating(rmodel)
model = DifferentialActionModelActuated(rmodel, actModel)
data = model.createData()
q = pinocchio.randomConfiguration(rmodel)
v = rand(rmodel.nv)
x = m2a(np.concatenate([q, v]))
u = m2a(rand(rmodel.nv - 6))
model.calcDiff(data, x, u)
mnum = DifferentialActionModelNumDiff(model)
dnum = mnum.createData()
mnum.calcDiff(dnum, x, u)
assertNumDiff(data.Fx, dnum.Fx,
NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 2.7e-2, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(data.Fu, dnum.Fu,
NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 7e-3, is now 2.11e-4 (see assertNumDiff.__doc__)
costModel.calcDiff(costData, x, u)
costModelND = CostModelNumDiff(
costModel,
StatePinocchio(rmodel),
withGaussApprox=True,
reevals=[
lambda m, d, x, u: pinocchio.forwardKinematics(m, d, a2m(x[:rmodel.nq]), a2m(x[rmodel.nq:])),
lambda m, d, x, u: pinocchio.computeJointJacobians(m, d, a2m(x[:rmodel.nq])),
lambda m, d, x, u: pinocchio.updateFramePlacements(m, d)
])
costDataND = costModelND.createData(rdata)
costModelND.calcDiff(costDataND, x, u)
assertNumDiff(costData.Lx, costDataND.Lx, NUMDIFF_MODIFIER *
costModelND.disturbance) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(costData.Lu, costDataND.Lu, NUMDIFF_MODIFIER *
costModelND.disturbance) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(costData.Lxx, costDataND.Lxx, NUMDIFF_MODIFIER *
costModelND.disturbance) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(costData.Lxu, costDataND.Lxu, NUMDIFF_MODIFIER *
costModelND.disturbance) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(costData.Luu, costDataND.Luu, NUMDIFF_MODIFIER *
costModelND.disturbance) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__)
q = pinocchio.randomConfiguration(rmodel)
v = rand(rmodel.nv)
x = m2a(np.concatenate([q, v]))
u = m2a(rand(rmodel.nv))
costModel = CostModelFrameRotation(rmodel, rmodel.getFrameId('gripper_left_fingertip_2_link'), np.eye(3))
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,
rmodel.getFrameId('gripper_left_fingertip_2_link'),
ref=np.random.rand(3),
gains=[4., 4.])
def df(am, ad, x):
return (am.calc(ad, x + h) - am.calc(ad, x)) / h
def ddf(am, ad, x):
return (am.calcDiff(ad, x + h)[0] - am.calcDiff(ad, x)[0]) / h
am = ActivationModelQuad()
ad = am.createData()
x = np.random.rand(1)
am.calc(ad, x)
assertNumDiff(
df(am, ad, x),
am.calcDiff(ad, x)[0],
1e-6) # threshold was 1e-6, is now 1e-6 (see assertNumDiff.__doc__)
assertNumDiff(
ddf(am, ad, x),
am.calcDiff(ad, x)[1],
1e-6) # threshold was 1e-6, is now 1e-6 (see assertNumDiff.__doc__)
am = ActivationModelWeightedQuad(np.random.rand(1))
ad = am.createData()
assertNumDiff(
df(am, ad, x),
am.calcDiff(ad, x)[0],
1e-6) # threshold was 1e-6, is now 1e-6 (see assertNumDiff.__doc__)
assertNumDiff(
ddf(am, ad, x),
am.calcDiff(ad, x)[1],
def dy_dq(i):
return df_dx(lambda _q: get_y(_q, a2m(x[nq:]))[i], a2m(x[:nq]))
def dy_dv(i):
return df_dx(lambda _v: get_y(a2m(x[:nq]), _v)[i], a2m(x[nq:]))
def e_k(i):
return data.dki_dx[i][:, :nv] - dk_dq(i)
assertNumDiff(
data.Fu, dxn_du, 1e4 * mnum.disturbance
) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__)
for i in range(4):
assertNumDiff(
data.dki_du[i], dk_du(i), 1e4 * mnum.disturbance
) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__)
for i in range(4):
assertNumDiff(
data.dki_dx[i][:, :nv], dk_dq(i), 1e4 * mnum.disturbance
) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(
data.dki_dx[i][:, nv:], dk_dv(i), 1e4 * mnum.disturbance
) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__)
for i in range(4):
def test_partial_derivatives_against_numdiff(self):
# Generating random values for the state and control
x = self.MODEL.State.rand()
u = np.random.rand(self.MODEL.nu)
# Computing the action derivatives
self.MODEL.calcDiff(self.DATA, x, u)
self.MODEL_NUMDIFF.calcDiff(self.DATA_NUMDIFF, x, u)
# Checking the partial derivatives against NumDiff
tol = self.NUMDIFF_MOD * self.MODEL_NUMDIFF.disturbance
assertNumDiff(self.DATA.Fx, self.DATA_NUMDIFF.Fx, tol)
assertNumDiff(self.DATA.Fu, self.DATA_NUMDIFF.Fu, tol)
assertNumDiff(self.DATA.Lx, self.DATA_NUMDIFF.Lx, tol)
assertNumDiff(self.DATA.Lu, self.DATA_NUMDIFF.Lu, tol)
assertNumDiff(self.DATA.Lxx, self.DATA_NUMDIFF.Lxx, tol)
assertNumDiff(self.DATA.Lxu, self.DATA_NUMDIFF.Lxu, tol)
assertNumDiff(self.DATA.Luu, self.DATA_NUMDIFF.Luu, tol)