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)