def __init__(self):
robot = self.robot = loadTalosArm(freeFloating=True)
rmodel = self.rmodel = robot.model
qmin = rmodel.lowerPositionLimit
qmin[:7] = -1
rmodel.lowerPositionLimit = qmin
qmax = rmodel.upperPositionLimit
qmax[:7] = 1
rmodel.upperPositionLimit = qmax
State = self.State = StatePinocchio(rmodel)
actModel = self.actModel = ActuationModelFreeFloating(rmodel)
contactModel = self.contactModel = ContactModelMultiple(rmodel)
contact6 = ContactModel6D(rmodel,
rmodel.getFrameId(contactName),
ref=pinocchio.SE3.Identity(),
gains=[0., 0.])
contactModel.addContact(name='contact', contact=contact6)
costModel = self.costModel = CostModelSum(rmodel, nu=actModel.nu)
self.cost1 = CostModelFrameTranslation(
rmodel,
nu=actModel.nu,
frame=rmodel.getFrameId(opPointName),
ref=np.array([.5, .4, .3]))
stateWeights = np.array([0] * 6 + [0.01] * (rmodel.nv - 6) +
[10] * rmodel.nv)
self.cost2 = CostModelState(rmodel,
State,
ref=State.zero(),
nu=actModel.nu,
activation=ActivationModelWeightedQuad(
stateWeights**2))
self.cost3 = CostModelControl(rmodel, nu=actModel.nu)
costModel.addCost(name="pos", weight=10, cost=self.cost1)
costModel.addCost(name="regx", weight=0.1, cost=self.cost2)
costModel.addCost(name="regu", weight=0.01, cost=self.cost3)
self.dmodel = DifferentialActionModelFloatingInContact(
rmodel, actModel, contactModel, costModel)
self.model = IntegratedActionModelEuler(self.dmodel)
self.data = self.model.createData()
self.cd1 = self.data.differential.costs.costs['pos']
self.cd2 = self.data.differential.costs.costs['regx']
self.cd3 = self.data.differential.costs.costs['regu']
self.ddata = self.data.differential
self.rdata = self.data.differential.pinocchio
self.x = self.State.rand()
self.q = a2m(self.x[:rmodel.nq])
self.v = a2m(self.x[rmodel.nq:])
self.u = np.random.rand(self.model.nu)