x = np.concatenate([m2a(q), m2a(v)])
u = np.random.rand(rmodel.nv - 6) * 2 - 1
actModel = ActuationModelFreeFloating(rmodel)
contactModel3 = ContactModel3D(
rmodel,
rmodel.getFrameId('gripper_left_fingertip_2_link'),
ref=np.random.rand(3),
gains=[4., 4.])
rmodel.frames[contactModel3.frame].placement = pinocchio.SE3.Random()
contactModel = ContactModelMultiple(rmodel)
contactModel.addContact(name='fingertip', contact=contactModel3)
model = DifferentialActionModelFloatingInContact(rmodel, actModel,
contactModel,
CostModelSum(rmodel))
data = model.createData()
model.calc(data, x, u)
assert (len(list(filter(lambda x: x > 0, eig(data.K)[0]))) == model.nv)
assert (len(list(filter(lambda x: x < 0, eig(data.K)[0]))) == model.ncontact)
_taucheck = pinocchio.rnea(rmodel, rdata, q, v, a2m(data.a),
data.contact.forces)
_taucheck.flat[:] += rmodel.armature.flat * data.a
assert (absmax(_taucheck[:6]) < 1e-6)
assert (absmax(m2a(_taucheck[6:]) - u) < 1e-6)
model.calcDiff(data, x, u)
mnum = DifferentialActionModelNumDiff(model, withGaussApprox=False)
np.set_printoptions(linewidth=400, suppress=True)
State = StatePinocchio(rmodel)
actModel = ActuationModelFreeFloating(State)
gains = pinocchio.utils.rand(2)
Mref_lf = FramePlacement(rmodel.getFrameId('LF_FOOT'), pinocchio.SE3.Random())
contactModel6 = ContactModel6D(State, Mref_lf, actModel.nu, gains)
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), 0.,
True)
data = model.createData()
model.calc(data, x, u)
model.calcDiff(data, x, u)
mnum = DifferentialActionModelNumDiff(model, False)
dnum = mnum.createData()
mnum.calc(dnum, x, u)
mnum.calcDiff(dnum, x, u)
model.costs.addCost("momentum", CostModelMomentum(State, a2m(np.random.rand(6)), actModel.nu), 1.)
data = model.createData()