def setUp(self):
# Creating NumDiff action model
self.MODEL_NUMDIFF = ActionModelNumDiff(self.MODEL,
withGaussApprox=True)
# Creating the action data
self.DATA = self.MODEL.createData()
self.DATA_NUMDIFF = self.MODEL_NUMDIFF.createData()
class ActionModelTestCase(unittest.TestCase):
MODEL = None
MODEL_NUMDIFF = None
NUMDIFF_MOD = NUMDIFF_MODIFIER
def setUp(self):
# Creating NumDiff action model
self.MODEL_NUMDIFF = ActionModelNumDiff(self.MODEL,
withGaussApprox=True)
# Creating the action data
self.DATA = self.MODEL.createData()
self.DATA_NUMDIFF = self.MODEL_NUMDIFF.createData()
def test_calc_retunrs_state(self):
# Generating random state and control vectors
x = self.MODEL.State.rand()
u = np.random.rand(self.MODEL.nu)
# Getting the state dimension from calc() call
nx = self.MODEL.calc(self.DATA, x, u)[0].shape
# Checking the dimension for the state and its tangent
self.assertEqual(nx, (self.MODEL.nx, ),
"Dimension of state vector is wrong.")
def test_calc_returns_a_cost(self):
# Getting the cost value computed by calc()
x = self.MODEL.State.rand()
u = np.random.rand(self.MODEL.nu)
cost = self.MODEL.calc(self.DATA, x, u)[1]
# Checking that calc returns a cost value
self.assertTrue(isinstance(cost, float),
"calc() doesn't return a cost value.")
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)
impulseModel = ImpulseModel6D(rmodel, rmodel.getFrameId(contactName))
costModel = CostModelImpactWholeBody(rmodel)
model = ActionModelImpact(rmodel, impulseModel, costModel)
model.impulseWeight = 1.
data = model.createData()
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__)
ddata = dmodel.createData()
model = IntegratedActionModelEuler(dmodel)
data = model.createData()
x = model.State.zero()
u = np.zeros(model.nu)
xn, c = model.calc(data, x, u)
model.timeStep = 1
model.differential.costs
for k in model.differential.costs.costs.keys():
model.differential.costs[k].weight = 1
model.calcDiff(data, x, u)
mnum = ActionModelNumDiff(model, withGaussApprox=True)
dnum = mnum.createData()
mnum.calcDiff(dnum, x, u)
assertNumDiff(data.Fx, dnum.Fx,
NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 3.16e-2, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(data.Fu, dnum.Fu,
NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 3.16e-2, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(data.Lx, dnum.Lx,
NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 3.16e-2, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(data.Lu, dnum.Lu,
NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 3.16e-2, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(dnum.Lxx, data.Lxx,
NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 3.16e-2, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(dnum.Lxu, data.Lxu,
NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 3.16e-2, is now 2.11e-4 (see assertNumDiff.__doc__)
nv = nq
dmodel = DifferentialActionModelLQR(nq, nu, driftFree=False)
ddata = dmodel.createData()
model = IntegratedActionModelRK4(dmodel)
data = model.createData()
x = model.State.rand()
# u = np.random.rand( model.nu )
u = rand(model.nu)
xn, c = model.calc(data, x, u)
model.timeStep = 1
mnum = ActionModelNumDiff(model, withGaussApprox=False)
dnum = mnum.createData()
model.calcDiff(data, x, u)
def get_k(q, v):
x_ = np.vstack([q, v])
model.calc(data, m2a(x_), u)
return [a2m(ki) for ki in data.ki]
def get_ku(u):
model.calc(data, x, m2a(u))
return [a2m(ki) for ki in data.ki]