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__)