rmodel.frames[Mref_lf.frame].placement = pinocchio.SE3.Random()
contactModel = ContactModelMultiple(State, actModel.nu)
contactModel.addContact("LF_FOOT_contact", contactModel6)
contactData = contactModel.createData(rdata)
model = DifferentialActionModelFloatingInContact(
State, actModel, contactModel, CostModelSum(State, actModel.nu, False), 0.,
True)
data = model.createData()
model.calc(data, x, u)
model.calcDiff(data, x, u)
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)
cost=CostModelState(pinocchioModel, self.State, self.State.zero(), activation=act1))
self.costs.addCost(name="regu", weight=0.01, cost=CostModelControl(pinocchioModel))
q = m2a(pinocchio.randomConfiguration(rmodel))
v = np.random.rand(rmodel.nv) * 2 - 1
x = np.concatenate([q, v])
u = np.random.rand(rmodel.nv) * 2 - 1
model = DifferentialActionModelPositioning(rmodel)
data = model.createData()
a, _ = model.calc(data, x, u)
model.calcDiff(data, x, u)
mnum = DifferentialActionModelNumDiff(model, withGaussApprox=True)
dnum = mnum.createData()
model.calcDiff(data, x, u)
mnum.calcDiff(dnum, x, u)
assertNumDiff(data.Fx, dnum.Fx,
NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(data.Fu, dnum.Fu,
NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(data.Rx, dnum.Rx,
NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(data.Ru, dnum.Ru,
NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__)
dmodel = DifferentialActionModelPositioning(rmodel)
ddata = dmodel.createData()