| """ |
| 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 = """ |
| <mujoco model="grasp_demo"> |
| <option timestep="0.002" gravity="0 0 -9.81"/> |
| |
| <visual> |
| <global offwidth="{w}" offheight="{h}"/> |
| <quality shadowsize="4096"/> |
| <headlight ambient="0.3 0.3 0.3" diffuse="0.6 0.6 0.6"/> |
| </visual> |
| |
| <asset> |
| <texture name="grid" type="2d" builtin="checker" rgb1="0.35 0.35 0.38" rgb2="0.30 0.30 0.33" |
| width="256" height="256"/> |
| <material name="grid_mat" texture="grid" texrepeat="8 8"/> |
| <material name="wood_mat" rgba="0.45 0.35 0.25 1"/> |
| <material name="finger_mat" rgba="0.3 0.3 0.35 1" specular="0.6" shininess="0.8"/> |
| <material name="obj_mat" rgba="{r} {g} {b} 1" specular="0.3" shininess="0.5"/> |
| </asset> |
| |
| <worldbody> |
| <light pos="0 -0.4 1.5" dir="0 0.3 -1" castshadow="true"/> |
| <light pos="0.4 0.3 1.0" dir="-0.2 -0.2 -1" castshadow="false" diffuse="0.3 0.3 0.35"/> |
| |
| <geom name="floor" type="plane" size="2 2 0.1" material="grid_mat"/> |
| |
| <body name="table" pos="0 0 0.38"> |
| <geom type="box" size="0.3 0.2 0.02" material="wood_mat"/> |
| <geom type="cylinder" size="0.012 0.18" pos="-0.26 -0.16 -0.2" rgba="0.4 0.3 0.2 1"/> |
| <geom type="cylinder" size="0.012 0.18" pos=" 0.26 -0.16 -0.2" rgba="0.4 0.3 0.2 1"/> |
| <geom type="cylinder" size="0.012 0.18" pos="-0.26 0.16 -0.2" rgba="0.4 0.3 0.2 1"/> |
| <geom type="cylinder" size="0.012 0.18" pos=" 0.26 0.16 -0.2" rgba="0.4 0.3 0.2 1"/> |
| </body> |
| |
| <!-- Object to grasp --> |
| <body name="object" pos="0 0 0.435"> |
| <joint type="free"/> |
| <geom name="obj_geom" type="{shape}" size="{size}" mass="{mass}" material="obj_mat" |
| friction="{friction} 0.005 0.0001"/> |
| </body> |
| |
| <!-- Left finger --> |
| <body name="finger_left" pos="0 -0.06 0.435"> |
| <joint name="finger_left_j" type="slide" axis="0 1 0" range="-0.1 0.1"/> |
| <geom type="box" size="0.025 0.008 0.03" material="finger_mat"/> |
| </body> |
| |
| <!-- Right finger --> |
| <body name="finger_right" pos="0 0.06 0.435"> |
| <joint name="finger_right_j" type="slide" axis="0 -1 0" range="-0.1 0.1"/> |
| <geom type="box" size="0.025 0.008 0.03" material="finger_mat"/> |
| </body> |
| |
| <!-- Lift mechanism (invisible) --> |
| <body name="lift_bar" pos="0 0 0.5"> |
| <joint name="lift_j" type="slide" axis="0 0 1" range="-0.2 0.3"/> |
| <geom type="box" size="0.001 0.001 0.001" rgba="0 0 0 0"/> |
| </body> |
| |
| <camera name="main" pos="0.15 -0.4 0.6" xyaxes="1 0 0 0 0.5 0.87" fovy="45"/> |
| <camera name="front" pos="0 -0.35 0.5" xyaxes="1 0 0 0 0.45 0.89" fovy="40"/> |
| </worldbody> |
| |
| <actuator> |
| <position joint="finger_left_j" kp="200" ctrlrange="-0.1 0.1"/> |
| <position joint="finger_right_j" kp="200" ctrlrange="-0.1 0.1"/> |
| <position joint="lift_j" kp="500" ctrlrange="-0.2 0.3"/> |
| </actuator> |
| </mujoco> |
| """ |
|
|
| 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) |
| |
| |
| for _ in range(200): |
| mujoco.mj_step(model, data) |
| |
| frames = [] |
| frames_per_step = max(1, int(1.0 / (FPS * model.opt.timestep))) |
| |
| |
| for _ in range(int(0.5 / model.opt.timestep)): |
| data.ctrl[0] = 0 |
| data.ctrl[1] = 0 |
| data.ctrl[2] = 0 |
| mujoco.mj_step(model, data) |
| if _ % frames_per_step == 0: |
| renderer.update_scene(data, camera=cam_id) |
| frames.append(renderer.render().copy()) |
| |
| |
| |
| |
| 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()) |
| |
| |
| 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 |
| 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"] |
| |
| |
| 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) |
| |
| |
| 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") |
| |
| |
| 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) |
| |
| |
| 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" |
| ) |
| |
| |
| print("\n--- Comparison: egg at multiple forces ---") |
| render_grasp_comparison("egg", [0.5, 1.5, 5.0], "exp3_egg_comparison.mp4") |
| |
| print("\nDone!") |
|
|
