def run_calcDiff_numDiff(epsilon) :
Lx = 0
Lx_err = 0
Lu = 0
Lu_err = 0
Lxx = 0
Lxx_err = 0
Luu = 0
Luu_err = 0
Luu_noFri = 0
Luu_err_noFri = 0
Fx = 0
Fx_err = 0
Fu = 0
Fu_err = 0
for k in range(N_trial):
N_iteration = random.randint(0, int(N_SIMULATION/k_mpc) - 1 )
N_model = random.randint(0, np.int(T_gait/dt_mpc)- 1 )
x = a + (b-a)*np.random.rand(12)
u = a + (b-a)*np.random.rand(12)
mpc_wrapper_ddp.updateProblem(logger_ddp.fsteps[:,:,N_iteration] , logger_ddp.xref[:,:, N_iteration] )
actionModel = mpc_wrapper_ddp.ListAction[N_model]
model_diff = crocoddyl.ActionModelNumDiff(actionModel)
# Run calc & calcDiff function : numDiff
model_diff.calc(data , x , u )
model_diff.calcDiff(data , x , u )
# Run calc & calcDiff function : c++
actionModel.calc(dataCpp , x , u )
actionModel.calcDiff(dataCpp , x , u )
Lx += np.sum( abs((data.Lx - dataCpp.Lx )) >= epsilon )
Lx_err += np.sum( abs((data.Lx - dataCpp.Lx )) )
Lu += np.sum( abs((data.Lu - dataCpp.Lu )) >= epsilon )
Lu_err += np.sum( abs((data.Lu - dataCpp.Lu )) )
Lxx += np.sum( abs((data.Lxx - dataCpp.Lxx )) >= epsilon )
Lxx_err += np.sum( abs((data.Lxx - dataCpp.Lxx )) )
Luu += np.sum( abs((data.Luu - dataCpp.Luu )) >= epsilon )
Luu_err += np.sum( abs((data.Luu - dataCpp.Luu )) )
Fx += np.sum( abs((data.Fx - dataCpp.Fx )) >= epsilon )
Fx_err += np.sum( abs((data.Fx - dataCpp.Fx )) )
Fu += np.sum( abs((data.Fu - dataCpp.Fu )) >= epsilon )
Fu_err += np.sum( abs((data.Fu - dataCpp.Fu )) )
# No friction cone :
actionModel.frictionWeights = 0.0
model_diff = crocoddyl.ActionModelNumDiff(actionModel)
# Run calc & calcDiff function : numDiff
model_diff.calc(data , x , u )
model_diff.calcDiff(data , x , u )
# Run calc & calcDiff function : c++
actionModel.calc(dataCpp , x , u )
actionModel.calcDiff(dataCpp , x , u )
Luu_noFri += np.sum( abs((data.Luu - dataCpp.Luu )) >= epsilon )
Luu_err_noFri += np.sum( abs((data.Luu - dataCpp.Luu )) )
Lx_err = Lx_err /N_trial
Lu_err = Lu_err/N_trial
Lxx_err = Lxx_err/N_trial
Luu_err = Luu_err/N_trial
Fx_err = Fx_err/N_trial
Fu_err = Fu_err/N_trial
return Lx , Lx_err , Lu , Lu_err , Lxx , Lxx_err , Luu , Luu_err, Luu_noFri , Luu_err_noFri, Fx, Fx_err, Fu , Fu_err
IMPULSES = crocoddyl.ImpulseModelMultiple(ROBOT_STATE)
IMPULSE_6D = crocoddyl.ImpulseModel6D(ROBOT_STATE,
ROBOT_MODEL.getFrameId('r_sole'))
IMPULSE_3D = crocoddyl.ImpulseModel3D(ROBOT_STATE,
ROBOT_MODEL.getFrameId('l_sole'))
IMPULSES.addImpulse("r_sole_impulse", IMPULSE_6D)
IMPULSES.addImpulse("l_sole_impulse", IMPULSE_3D)
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__)
a = 1 b = -1 N_trial = 10 epsilon = 10e-6 ################################################ ## CHECK DERIVATIVE WITH NUM_DIFF ################################################# model_diff = crocoddyl.ActionModelNumDiff(mpc_wrapper_ddp.ListAction[0]) data = model_diff.createData() dataCpp = mpc_wrapper_ddp.ListAction[0].createData() # RUN CALC DIFF def run_calcDiff_numDiff(epsilon) : Lx = 0 Lx_err = 0 Lu = 0 Lu_err = 0 Lxx = 0 Lxx_err = 0 Luu = 0 Luu_err = 0 Luu_noFri = 0 Luu_err_noFri = 0
