"""Double pendulum with energy-shaping control — modern MuJoCo example.""" import numpy as np import mujoco import mujoco.viewer XML = """ """ def main(): model = mujoco.MjModel.from_xml_string(XML) data = mujoco.MjData(model) # Target: both joints at 0 (hanging down) # Try to swing up: target is pi (upright) target = np.array([np.pi, 0.0]) kp = np.array([100.0, 50.0]) kd = np.array([20.0, 10.0]) with mujoco.viewer.launch_passive(model, data) as viewer: while viewer.is_running() and data.time < 15.0: # PD control to target error = target - data.qpos[:2] data.ctrl[:] = kp * error - kd * data.qvel[:2] # Clamp to actuator limits data.ctrl[:] = np.clip(data.ctrl, [-20, -10], [20, 10]) mujoco.mj_step(model, data) viewer.sync() if __name__ == "__main__": main()