|
| 1 | +"""Make the floating-base Pupper stand still under PD position servo. |
| 2 | +
|
| 3 | +Run from this directory with: |
| 4 | + mjpython stand_pupper_v3_floating.py # macOS |
| 5 | + python stand_pupper_v3_floating.py # Linux / Windows |
| 6 | +
|
| 7 | +This extends view_pupper_v3_floating.py with two pieces: |
| 8 | +
|
| 9 | +* A target pose STAND_POSE that is written into ``data.ctrl`` every step, so |
| 10 | + the position servo pulls the legs into a "knee-bent, body up" stand instead |
| 11 | + of just collapsing to the home (all zeros) pose. |
| 12 | +* Recording ``data.qpos[2]`` (base z) over time, so when the viewer is closed |
| 13 | + we print the final height and the last-1-second standard deviation as the |
| 14 | + Lab 4.5 pass criterion (std < 5 mm). |
| 15 | +
|
| 16 | +macOS note: do not run ``mjpython -m mujoco.viewer --mjcf=...``; see the |
| 17 | +docstring of view_pupper_v3_floating.py for the underlying mjpython + runpy |
| 18 | +issue. |
| 19 | +""" |
| 20 | + |
| 21 | +from __future__ import annotations |
| 22 | + |
| 23 | +import pathlib |
| 24 | +import sys |
| 25 | +import time |
| 26 | + |
| 27 | +import numpy as np |
| 28 | + |
| 29 | +import mujoco |
| 30 | +import mujoco.viewer |
| 31 | + |
| 32 | + |
| 33 | +_DIR = pathlib.Path(__file__).parent |
| 34 | + |
| 35 | +MODEL_PATH = _DIR / "models" / "pupper_v3_floating.xml" |
| 36 | + |
| 37 | +# Actuator order: front_r {1,2,3}, front_l {1,2,3}, back_r {1,2,3}, back_l {1,2,3}. |
| 38 | +# |
| 39 | +# Pupper's mesh + per-body quaternions are set up so that joint angle = 0 |
| 40 | +# already puts each leg into a knee-bent, body-up stance. That means the |
| 41 | +# stand pose is simply the home keyframe's ctrl (all zeros). We still write |
| 42 | +# it into data.ctrl every step so the servo target stays locked even when |
| 43 | +# external code (e.g. the §5 gait controller) starts touching data.ctrl in |
| 44 | +# the same loop. |
| 45 | +# |
| 46 | +# To experiment with other postures, change any of these 12 entries. Remember |
| 47 | +# that the left-side HFE / KFE limits are mirrored (see §4.2.1 in the chapter), |
| 48 | +# so non-zero left-leg targets typically need flipped signs. |
| 49 | +STAND_POSE = np.zeros(12, dtype=np.float64) |
| 50 | + |
| 51 | + |
| 52 | +def _load_model(path: pathlib.Path) -> tuple[mujoco.MjModel, mujoco.MjData]: |
| 53 | + """Load the MJCF model and put it into the home keyframe if available.""" |
| 54 | + model_path = path.expanduser().resolve() |
| 55 | + model = mujoco.MjModel.from_xml_path(str(model_path)) |
| 56 | + data = mujoco.MjData(model) |
| 57 | + |
| 58 | + home_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_KEY, "home") |
| 59 | + if home_id >= 0: |
| 60 | + mujoco.mj_resetDataKeyframe(model, data, home_id) |
| 61 | + else: |
| 62 | + mujoco.mj_resetData(model, data) |
| 63 | + |
| 64 | + mujoco.mj_forward(model, data) |
| 65 | + return model, data |
| 66 | + |
| 67 | + |
| 68 | +def _configure_viewer(viewer: mujoco.viewer.Handle) -> None: |
| 69 | + viewer.cam.lookat[:] = [0.0, 0.0, 0.15] |
| 70 | + viewer.cam.distance = 0.8 |
| 71 | + viewer.cam.azimuth = 135 |
| 72 | + viewer.cam.elevation = -22 |
| 73 | + |
| 74 | + |
| 75 | +def _report_stability(heights: list[float], timestep: float) -> None: |
| 76 | + """Print final base z and the last-1-second standard deviation.""" |
| 77 | + if not heights: |
| 78 | + print("No samples recorded; nothing to report.", file=sys.stderr) |
| 79 | + return |
| 80 | + samples_per_second = max(int(round(1.0 / timestep)), 1) |
| 81 | + tail = np.array(heights[-samples_per_second:]) |
| 82 | + print( |
| 83 | + f"final z={tail[-1]:.3f} m, " |
| 84 | + f"last-1s std={np.std(tail) * 1000.0:.2f} mm " |
| 85 | + f"(< 5 mm = stable)" |
| 86 | + ) |
| 87 | + |
| 88 | + |
| 89 | +def main() -> int: |
| 90 | + model_path = MODEL_PATH.resolve() |
| 91 | + model, data = _load_model(model_path) |
| 92 | + |
| 93 | + print(f"Loaded: {model_path}", flush=True) |
| 94 | + print( |
| 95 | + f" nq={model.nq}, nv={model.nv}, nu={model.nu}, nbody={model.nbody}", |
| 96 | + flush=True, |
| 97 | + ) |
| 98 | + print(f" timestep={model.opt.timestep:.4f} s", flush=True) |
| 99 | + print( |
| 100 | + "Opening viewer. Close the window to exit and see the stability report.", |
| 101 | + flush=True, |
| 102 | + ) |
| 103 | + |
| 104 | + heights: list[float] = [] |
| 105 | + try: |
| 106 | + with mujoco.viewer.launch_passive(model, data) as viewer: |
| 107 | + _configure_viewer(viewer) |
| 108 | + while viewer.is_running(): |
| 109 | + step_start = time.perf_counter() |
| 110 | + data.ctrl[:] = STAND_POSE |
| 111 | + mujoco.mj_step(model, data) |
| 112 | + heights.append(float(data.qpos[2])) |
| 113 | + viewer.sync() |
| 114 | + |
| 115 | + elapsed = time.perf_counter() - step_start |
| 116 | + if elapsed < model.opt.timestep: |
| 117 | + time.sleep(model.opt.timestep - elapsed) |
| 118 | + except RuntimeError as exc: |
| 119 | + if sys.platform == "darwin" and "mjpython" in str(exc): |
| 120 | + print("\nOn macOS, run the interactive viewer with mjpython:", file=sys.stderr) |
| 121 | + print(f" mjpython {pathlib.Path(__file__)}", file=sys.stderr) |
| 122 | + return 2 |
| 123 | + raise |
| 124 | + |
| 125 | + _report_stability(heights, model.opt.timestep) |
| 126 | + return 0 |
| 127 | + |
| 128 | + |
| 129 | +if __name__ == "__main__": |
| 130 | + raise SystemExit(main()) |
0 commit comments