mnum = DifferentialActionModelNumDiff(model, False)
dnum = mnum.createData()
mnum.calcDiff(dnum, x, u)
model.costs.addCost(
"momentum", CostModelMomentum(State, a2m(np.random.rand(6)), actModel.nu),
1.)
data = model.createData()
cmodel = model.costs.costs['momentum'].cost
cdata = data.costs.costs['momentum']
model.calcDiff(data, x, u)
mnum = DifferentialActionModelNumDiff(model, False)
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 (se
assertNumDiff(
data.Lx, dnum.Lx, NUMDIFF_MODIFIER * mnum.disturbance
) # threshold was 2.7e-2, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(data.Lu, dnum.Lu, NUMDIFF_MODIFIER *
mnum.disturbance) # threshold was 7e-3, is now 2.11e-4 (se
COSTS = crocoddyl.CostModelSum(ROBOT_STATE, 0)
COSTS.addCost("impulse_com", crocoddyl.CostModelImpulseCoM(ROBOT_STATE), 1.)
MODEL = crocoddyl.ActionModelImpulseFwdDynamics(ROBOT_STATE, IMPULSES, COSTS,
0., 0., True)
DATA = MODEL.createData()
# Created DAM numdiff
MODEL_ND = crocoddyl.ActionModelNumDiff(MODEL)
MODEL_ND.disturbance *= 10
dnum = MODEL_ND.createData()
x = ROBOT_STATE.rand()
u = pinocchio.utils.rand(0) # (ACTUATION.nu)
MODEL.calc(DATA, x, u)
MODEL.calcDiff(DATA, x, u)
MODEL_ND.calc(dnum, x, u)
MODEL_ND.calcDiff(dnum, x, u)
assertNumDiff(
DATA.Fx, dnum.Fx, NUMDIFF_MODIFIER * MODEL_ND.disturbance
) # threshold was 2.7e-2, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(
DATA.Fu, dnum.Fu, NUMDIFF_MODIFIER * MODEL_ND.disturbance
) # threshold was 2.7e-2, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(
DATA.Lx, dnum.Lx, NUMDIFF_MODIFIER * MODEL_ND.disturbance
) # threshold was 2.7e-2, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(
DATA.Lu, dnum.Lu, NUMDIFF_MODIFIER * MODEL_ND.disturbance
) # threshold was 2.7e-2, is now 2.11e-4 (see assertNumDiff.__doc__)
