()
| 19 | |
| 20 | @pytest.fixture |
| 21 | def mpc_problem(): |
| 22 | import pinocchio as pin |
| 23 | |
| 24 | rmodel = pin.buildSampleModelHumanoid() |
| 25 | rdata = rmodel.createData() |
| 26 | space = aligator.manifolds.MultibodyPhaseSpace(rmodel) |
| 27 | |
| 28 | dt = 1e-2 # Timestep |
| 29 | |
| 30 | # Contact models |
| 31 | FOOT_FRAME_IDS = { |
| 32 | fname: rmodel.getFrameId(fname) |
| 33 | for fname in ["lleg_effector_body", "rleg_effector_body"] |
| 34 | } |
| 35 | FOOT_JOINT_IDS = { |
| 36 | fname: rmodel.frames[fid].parentJoint for fname, fid in FOOT_FRAME_IDS.items() |
| 37 | } |
| 38 | |
| 39 | nv = rmodel.nv |
| 40 | nu = rmodel.nv |
| 41 | act_matrix = np.eye(nv, nu, -6) |
| 42 | prox_settings = pin.ProximalSettings(1e-9, 1e-10, 10) |
| 43 | constraint_models = [] |
| 44 | constraint_datas = [] |
| 45 | for fname, fid in FOOT_FRAME_IDS.items(): |
| 46 | joint_id = FOOT_JOINT_IDS[fname] |
| 47 | pl1 = rmodel.frames[fid].placement |
| 48 | pl2 = rdata.oMf[fid] |
| 49 | cm = pin.RigidConstraintModel( |
| 50 | pin.ContactType.CONTACT_3D, |
| 51 | rmodel, |
| 52 | joint_id, |
| 53 | pl1, |
| 54 | 0, |
| 55 | pl2, |
| 56 | pin.LOCAL, |
| 57 | ) |
| 58 | set_baumgarte_params(cm, 0.0, 0.0) |
| 59 | constraint_models.append(cm) |
| 60 | constraint_datas.append(cm.createData()) |
| 61 | |
| 62 | q0 = space.neutral() |
| 63 | x0 = np.concatenate((q0, np.zeros(nv))) |
| 64 | u0 = np.zeros(nu) |
| 65 | |
| 66 | T = 50 |
| 67 | |
| 68 | stages = [] |
| 69 | for _ in range(T): |
| 70 | rcost = aligator.CostStack(space, nu) |
| 71 | rcost.addCost(aligator.QuadraticStateCost(space, nu, x0, np.eye(space.ndx))) |
| 72 | rcost.addCost(aligator.QuadraticControlCost(space, u0, np.eye(nu) * 1e-4)) |
| 73 | |
| 74 | ode = aligator.dynamics.MultibodyConstraintFwdDynamics( |
| 75 | space, act_matrix, constraint_models, prox_settings |
| 76 | ) |
| 77 | dyn_model = aligator.dynamics.IntegratorSemiImplEuler(ode, dt) |
| 78 | stm = aligator.StageModel(rcost, dyn_model) |
nothing calls this directly
no test coverage detected