""" Render Experiment 3: Grasp Force Selection Simulates a parallel-jaw gripper closing on objects with different forces. Shows: too weak (drops), correct (lifts), too strong (crushes / deforms). """ import mujoco import numpy as np import imageio WIDTH, HEIGHT = 1280, 720 FPS = 60 OUTPUT_DIR = "." XML_GRASP = """ """ OBJECTS = { "egg": { "mass": 0.06, "friction": 0.3, "shape": "ellipsoid", "size": "0.02 0.02 0.027", "color": (0.95, 0.92, 0.85), "crush_force": 3.0, }, "rubber_ball": { "mass": 0.4, "friction": 0.8, "shape": "sphere", "size": "0.025", "color": (0.2, 0.2, 0.22), "crush_force": 50.0, }, "wood_block": { "mass": 0.5, "friction": 0.4, "shape": "box", "size": "0.025 0.025 0.025", "color": (0.65, 0.45, 0.25), "crush_force": 30.0, }, "glass_cup": { "mass": 0.3, "friction": 0.15, "shape": "cylinder", "size": "0.02 0.03", "color": (0.7, 0.85, 0.9), "crush_force": 8.0, }, } def render_grasp_attempt(obj_name, grip_force_target, filename, camera="front"): """Render a single grasp attempt: approach, squeeze, lift.""" obj = OBJECTS[obj_name] xml = XML_GRASP.format( w=WIDTH, h=HEIGHT, mass=obj["mass"], friction=obj["friction"], shape=obj["shape"], size=obj["size"], r=obj["color"][0], g=obj["color"][1], b=obj["color"][2], ) model = mujoco.MjModel.from_xml_string(xml) data = mujoco.MjData(model) renderer = mujoco.Renderer(model, height=HEIGHT, width=WIDTH) cam_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_CAMERA, camera) obj_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "object") mujoco.mj_forward(model, data) # Settle for _ in range(200): mujoco.mj_step(model, data) frames = [] frames_per_step = max(1, int(1.0 / (FPS * model.opt.timestep))) # Phase 1: Show object (0.5s) for _ in range(int(0.5 / model.opt.timestep)): data.ctrl[0] = 0 # fingers open data.ctrl[1] = 0 data.ctrl[2] = 0 # no lift mujoco.mj_step(model, data) if _ % frames_per_step == 0: renderer.update_scene(data, camera=cam_id) frames.append(renderer.render().copy()) # Phase 2: Close fingers (1s) # Map grip_force_target to finger position # Higher force = tighter grip = smaller gap finger_pos = min(0.06, 0.02 + 0.005 * grip_force_target / 10) close_steps = int(1.0 / model.opt.timestep) for step in range(close_steps): t = step / close_steps data.ctrl[0] = t * finger_pos data.ctrl[1] = t * finger_pos data.ctrl[2] = 0 mujoco.mj_step(model, data) if step % frames_per_step == 0: renderer.update_scene(data, camera=cam_id) frames.append(renderer.render().copy()) # Phase 3: Lift (1.5s) lift_steps = int(1.5 / model.opt.timestep) obj_z_before = data.xpos[obj_id][2] for step in range(lift_steps): t = step / lift_steps data.ctrl[0] = finger_pos data.ctrl[1] = finger_pos data.ctrl[2] = t * 0.15 # lift 15cm mujoco.mj_step(model, data) if step % frames_per_step == 0: renderer.update_scene(data, camera=cam_id) frames.append(renderer.render().copy()) obj_z_after = data.xpos[obj_id][2] lifted = (obj_z_after - obj_z_before) > 0.05 crushed = grip_force_target > obj["crush_force"] # Phase 4: Hold / show result (0.5s) for _ in range(int(0.5 / model.opt.timestep)): data.ctrl[2] = 0.15 mujoco.mj_step(model, data) if _ % frames_per_step == 0: renderer.update_scene(data, camera=cam_id) frames.append(renderer.render().copy()) output_path = f"{OUTPUT_DIR}/{filename}" writer = imageio.get_writer(output_path, fps=FPS, codec='libx264', output_params=['-crf', '20', '-preset', 'medium']) for frame in frames: writer.append_data(frame) writer.close() result = "LIFTED" if lifted else "DROPPED" if crushed: result = "CRUSHED (force > threshold)" print(f" {obj_name} @ {grip_force_target}N -> {result} | Saved: {output_path} ({len(frames)} frames)") renderer.close() def render_grasp_comparison(obj_name, forces, filename): """Render multiple grasp attempts at different forces, concatenated.""" obj = OBJECTS[obj_name] xml = XML_GRASP.format( w=WIDTH, h=HEIGHT, mass=obj["mass"], friction=obj["friction"], shape=obj["shape"], size=obj["size"], r=obj["color"][0], g=obj["color"][1], b=obj["color"][2], ) model = mujoco.MjModel.from_xml_string(xml) renderer = mujoco.Renderer(model, height=HEIGHT, width=WIDTH) cam_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_CAMERA, "front") all_frames = [] for force in forces: data = mujoco.MjData(model) mujoco.mj_forward(model, data) obj_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "object") for _ in range(200): mujoco.mj_step(model, data) frames_per_step = max(1, int(1.0 / (FPS * model.opt.timestep))) finger_pos = min(0.06, 0.02 + 0.005 * force / 10) # Quick approach + close + lift cycle phases = [ ("show", 0.3, lambda t: (0, 0, 0)), ("close", 0.6, lambda t: (t * finger_pos, t * finger_pos, 0)), ("lift", 1.0, lambda t: (finger_pos, finger_pos, t * 0.12)), ("hold", 0.3, lambda t: (finger_pos, finger_pos, 0.12)), ] for phase_name, dur, ctrl_fn in phases: steps = int(dur / model.opt.timestep) for step in range(steps): t = step / max(steps, 1) c = ctrl_fn(t) data.ctrl[0] = c[0] data.ctrl[1] = c[1] data.ctrl[2] = c[2] mujoco.mj_step(model, data) if step % frames_per_step == 0: renderer.update_scene(data, camera=cam_id) all_frames.append(renderer.render().copy()) output_path = f"{OUTPUT_DIR}/{filename}" writer = imageio.get_writer(output_path, fps=FPS, codec='libx264', output_params=['-crf', '20', '-preset', 'medium']) for frame in all_frames: writer.append_data(frame) writer.close() print(f" Saved: {output_path} ({len(all_frames)} frames, {len(all_frames)/FPS:.1f}s)") renderer.close() if __name__ == "__main__": print("=== Rendering Experiment 3: Grasp Force Selection ===\n") # Individual grasp attempts for obj_name in ["egg", "rubber_ball", "wood_block"]: print(f"\n--- {obj_name} ---") obj = OBJECTS[obj_name] min_f = obj["mass"] * 9.81 / (obj["friction"] * 2) # Three attempts: too weak, just right, too strong attempts = [ (min_f * 0.5, "weak"), (min_f * 1.5, "good"), (obj["crush_force"] * 1.5, "crush"), ] for force, label in attempts: render_grasp_attempt( obj_name, force, f"exp3_{obj_name}_{label}.mp4" ) # Comparison videos print("\n--- Comparison: egg at multiple forces ---") render_grasp_comparison("egg", [0.5, 1.5, 5.0], "exp3_egg_comparison.mp4") print("\nDone!")