"""Pick-and-place with a simple gripper — multi-body coordination example.""" import numpy as np import mujoco import mujoco.viewer XML = """ """ class PickAndPlaceController: """Waypoint-based pick-and-place state machine.""" def __init__(self): # Joint targets for key poses (shoulder, elbow, wrist, finger_l, finger_r) self.phases = [ # Phase 0: Open gripper, move above object {"joints": [0.6, -1.2, 0.6], "gripper": 0.03, "duration": 1.5}, # Phase 1: Lower to grasp {"joints": [0.8, -1.4, 0.6], "gripper": 0.03, "duration": 1.0}, # Phase 2: Close gripper {"joints": [0.8, -1.4, 0.6], "gripper": 0.0, "duration": 0.8}, # Phase 3: Lift {"joints": [0.4, -0.8, 0.4], "gripper": 0.0, "duration": 1.2}, # Phase 4: Move to place location {"joints": [-0.6, -1.0, 0.6], "gripper": 0.0, "duration": 1.5}, # Phase 5: Lower to place {"joints": [-0.8, -1.4, 0.6], "gripper": 0.0, "duration": 1.0}, # Phase 6: Release {"joints": [-0.8, -1.4, 0.6], "gripper": 0.03, "duration": 0.8}, # Phase 7: Retract {"joints": [0.0, -0.5, 0.0], "gripper": 0.03, "duration": 1.5}, ] self.phase = 0 self.timer = 0.0 def get_ctrl(self, dt): if self.phase >= len(self.phases): self.phase = len(self.phases) - 1 p = self.phases[self.phase] self.timer += dt ctrl = np.zeros(5) ctrl[0:3] = p["joints"] ctrl[3] = p["gripper"] ctrl[4] = p["gripper"] if self.timer >= p["duration"]: self.timer = 0.0 self.phase += 1 if self.phase < len(self.phases): print(f" Phase {self.phase}: {['above', 'lower', 'grasp', 'lift', 'move', 'place', 'release', 'retract'][self.phase]}") return ctrl def main(): model = mujoco.MjModel.from_xml_string(XML) data = mujoco.MjData(model) controller = PickAndPlaceController() dt = model.opt.timestep print("Pick-and-place demo") print(" Phase 0: moving above object") with mujoco.viewer.launch_passive(model, data) as viewer: while viewer.is_running() and data.time < 12.0: data.ctrl[:] = controller.get_ctrl(dt) mujoco.mj_step(model, data) viewer.sync() if __name__ == "__main__": main()