diff --git a/src/real2sim_platform/act_inference.py b/src/real2sim_platform/act_inference.py index f939eac..f661cd3 100644 --- a/src/real2sim_platform/act_inference.py +++ b/src/real2sim_platform/act_inference.py @@ -74,9 +74,8 @@ def infer( max_cube_z = settled_z bilateral_steps = 0 for step in range(steps): - runtime.renderer.update_scene(runtime.data, camera="overview") - frame = runtime.renderer.render().copy() - qpos = np.concatenate([runtime.data.qpos[runtime.qpos_adrs], [runtime.data.qpos[runtime.gripper_qadr]]]).astype(np.float32) + frame = runtime.capture_frame() + qpos = runtime.joint_state() normalized_qpos = (qpos - np.asarray(stats["qpos_mean"], dtype=np.float32)) / np.asarray(stats["qpos_std"], dtype=np.float32) image = torch.from_numpy(frame).permute(2, 0, 1).unsqueeze(0).float().cuda() / 255.0 qpos_tensor = torch.from_numpy(normalized_qpos).unsqueeze(0).float().cuda() @@ -87,28 +86,26 @@ def infer( action = np.asarray(action, dtype=np.float32) if action.shape != (7,): raise ValueError(f"ACT action shape must be (7,), received {action.shape}") - for index in range(7): - action[index] = np.clip(action[index], runtime.ctrl_ranges[index, 0], runtime.ctrl_ranges[index, 1]) - runtime.data.ctrl[:7] = action - for _ in range(CONTROL_HOLD_STEPS): - runtime.mujoco.mj_step(runtime.model, runtime.data) + action = runtime.clip_action(action) + runtime.apply_action(action) touching = runtime.finger_contacts() bilateral_steps += int(len(touching) == 2) - max_cube_z = max(max_cube_z, float(runtime.data.qpos[runtime.cube_qadr + 2])) + cube_position = runtime.cube_position() + max_cube_z = max(max_cube_z, float(cube_position[2])) all_actions.append(action.copy()) frames.append(frame) trajectory.append( { "step": step, - "time": float(runtime.data.time), + "time": runtime.simulation_time, "qpos": qpos.tolist(), "action": action.tolist(), - "cube_pos": runtime.data.qpos[runtime.cube_qadr : runtime.cube_qadr + 3].astype(float).tolist(), + "cube_pos": cube_position.tolist(), "target": target.tolist(), "bilateral_contact": len(touching) == 2, } ) - final_cube = runtime.data.qpos[runtime.cube_qadr : runtime.cube_qadr + 3].copy() + final_cube = runtime.cube_position() lift_height = float(max_cube_z - settled_z) place_error = float(np.linalg.norm(final_cube - target)) success = bilateral_steps > 0 and lift_height >= 0.10 and place_error <= 0.05 @@ -132,7 +129,7 @@ def infer( nonconstant = bool(len(action_matrix) > 1 and np.max(np.std(action_matrix, axis=0)) > 1e-5) report = { "schema": "real2sim.discoverse_act_inference.v1", - "runtime": "mujoco", + "runtime": "motrixsim", "scene": str(scene_path), "model": str(model_dir), "episodes": results, @@ -150,22 +147,11 @@ def infer( def _reset_randomized(runtime: RM65PickPlaceRuntime) -> tuple[np.ndarray, np.ndarray, float]: - runtime.mujoco.mj_resetDataKeyframe(runtime.model, runtime.data, 0) - cube = np.array([runtime.rng.uniform(0.645, 0.655), runtime.rng.uniform(-0.010, 0.010), 0.33], dtype=float) - target = np.array([runtime.rng.uniform(0.42, 0.51), runtime.rng.uniform(0.13, 0.22), 0.31], dtype=float) - runtime.data.qpos[runtime.cube_qadr : runtime.cube_qadr + 3] = cube - runtime.data.qpos[runtime.cube_qadr + 3 : runtime.cube_qadr + 7] = np.array([1.0, 0.0, 0.0, 0.0]) - cube_dof = int(runtime.model.jnt_dofadr[runtime.cube_joint]) - runtime.data.qvel[cube_dof : cube_dof + 6] = 0 - runtime.model.body_pos[runtime.target_body] = np.array([target[0], target[1], 0.335]) - runtime.mujoco.mj_forward(runtime.model, runtime.data) - for _ in range(120): - runtime.mujoco.mj_step(runtime.model, runtime.data) - return cube, target, float(runtime.data.qpos[runtime.cube_qadr + 2]) + return runtime.reset_randomized() def main() -> int: - parser = argparse.ArgumentParser(description="Run DISCOVERSE ACT in the physical RM65 MuJoCo scene") + parser = argparse.ArgumentParser(description="Run DISCOVERSE ACT in the physical RM65 MotrixSim scene") parser.add_argument("--scene", type=Path, required=True) parser.add_argument("--model", type=Path, required=True) parser.add_argument("--act-root", type=Path, required=True) diff --git a/src/real2sim_platform/adapters.py b/src/real2sim_platform/adapters.py index c9f0de6..4cedaa5 100644 --- a/src/real2sim_platform/adapters.py +++ b/src/real2sim_platform/adapters.py @@ -3062,6 +3062,9 @@ def _validate_motrixsim_asset(candidates: list[Path], *, level: str) -> dict[str data = SceneData(model) for _ in range(8): step(model, data) + if not descriptor["ok"]: + descriptor = _validate_sim_asset([runtime_scene], required_suffix=".xml") + descriptor.update({"artifact_kind": "mjcf", "role": "motrixsim_runtime_input"}) return { "ok": True, "path": str(runtime_scene), diff --git a/src/real2sim_platform/motrix_scene.py b/src/real2sim_platform/motrix_scene.py index 5b23f19..70a5fc5 100644 --- a/src/real2sim_platform/motrix_scene.py +++ b/src/real2sim_platform/motrix_scene.py @@ -760,15 +760,14 @@ def _append_composition_primitive( worldbody = root.find("worldbody") if worldbody is None: worldbody = ET.SubElement(root, "worldbody") - body = ET.SubElement( - worldbody, - "body", - { - "name": runtime_name, - "pos": _numbers(transform["translation"]), - "euler": _numbers([math.radians(value) for value in transform["rotation_euler_degrees"]]), - }, - ) + body_attributes = { + "name": runtime_name, + "pos": _numbers(transform["translation"]), + "euler": _numbers([math.radians(value) for value in transform["rotation_euler_degrees"]]), + } + if runtime_name == "place_target": + body_attributes["mocap"] = "true" + body = ET.SubElement(worldbody, "body", body_attributes) if physics["dynamic"]: joint_name = "pick_cube_free" if runtime_name == "pick_cube" else f"{runtime_name}_free" ET.SubElement(body, "freejoint", {"name": joint_name}) @@ -799,7 +798,7 @@ def _append_composition_primitive( "friction": _numbers(physics["friction"]), "contype": "0" if collision_mode == "visual_only" else "1", "conaffinity": "0" if collision_mode == "visual_only" else "1", - "group": "2" if collision_mode == "visual_only" else "3", + "group": "2" if collision_mode == "visual_only" else "0", "rgba": _numbers(rgba), **_primitive_geom(primitive, scaled_dimensions), } @@ -886,7 +885,7 @@ def _append_composition_mesh( "friction": _numbers(physics["friction"]), "contype": "0" if physics["collision_mode"] == "visual_only" else "1", "conaffinity": "0" if physics["collision_mode"] == "visual_only" else "1", - "group": "2" if physics["collision_mode"] == "visual_only" else "3", + "group": "2" if physics["collision_mode"] == "visual_only" else "0", "rgba": _numbers(physics["rgba"]), } if physics["dynamic"]: @@ -916,7 +915,7 @@ def _ensure_overview_camera(root: ET.Element, *, target: str) -> None: worldbody = ET.SubElement(root, "worldbody") existing = worldbody.find("./camera[@name='overview']") if existing is None: - ET.SubElement(worldbody, "camera", {"name": "overview", "mode": "targetbody", "target": target, "pos": "2 -2 1.8", "fovy": "55"}) + ET.SubElement(worldbody, "camera", {"name": "overview", "mode": "targetbody", "target": target, "pos": "1.65 -1.25 1.15", "fovy": "50"}) def _write_home_keyframe(runtime_scene: Path) -> None: diff --git a/src/real2sim_platform/rm65_controller.py b/src/real2sim_platform/rm65_controller.py index 3b63d76..d94f904 100644 --- a/src/real2sim_platform/rm65_controller.py +++ b/src/real2sim_platform/rm65_controller.py @@ -35,44 +35,95 @@ class EpisodeResult: class RM65PickPlaceRuntime: def __init__(self, scene_path: Path, *, width: int, height: int, seed: int) -> None: - import mujoco + import motrixsim + from motrixsim import ik + from motrixsim.render import RenderApp - self.mujoco = mujoco - self.model = mujoco.MjModel.from_xml_path(str(scene_path)) - self.data = mujoco.MjData(self.model) - self.renderer = mujoco.Renderer(self.model, height=height, width=width) + self.motrixsim = motrixsim + self.model = motrixsim.load_model(str(scene_path)) + self.data = motrixsim.SceneData(self.model) self.rng = np.random.default_rng(seed) - self.site_id = self._id(mujoco.mjtObj.mjOBJ_SITE, "gripper") - self.cube_joint = self._id(mujoco.mjtObj.mjOBJ_JOINT, "pick_cube_free") - self.cube_qadr = int(self.model.jnt_qposadr[self.cube_joint]) - self.cube_geom = self._id(mujoco.mjtObj.mjOBJ_GEOM, "pick_cube_geom") - self.finger_geoms = { - self._id(mujoco.mjtObj.mjOBJ_GEOM, "left_finger_pad_z_axis"), - self._id(mujoco.mjtObj.mjOBJ_GEOM, "right_finger_pad_z_axis"), - } - self.target_body = self._id(mujoco.mjtObj.mjOBJ_BODY, "place_target") - self.target_home = self.model.body_pos[self.target_body].astype(float).copy() - self.gripper_joint = self._id(mujoco.mjtObj.mjOBJ_JOINT, "gripper_Left_1_Joint") - self.gripper_qadr = int(self.model.jnt_qposadr[self.gripper_joint]) - self.joint_ids = [self._id(mujoco.mjtObj.mjOBJ_JOINT, f"joint_{index}") for index in range(1, 7)] - self.qpos_adrs = [int(self.model.jnt_qposadr[joint]) for joint in self.joint_ids] - self.dof_adrs = [int(self.model.jnt_dofadr[joint]) for joint in self.joint_ids] - self.ctrl_ranges = self.model.actuator_ctrlrange.copy() + self.gripper_site = self._require(self.model.get_site("gripper"), "site gripper") + self.finger_sites = [ + self._require(self.model.get_site("left_finger_pad"), "site left_finger_pad"), + self._require(self.model.get_site("right_finger_pad"), "site right_finger_pad"), + ] + self.cube_body = self._require(self.model.get_body("pick_cube"), "body pick_cube") + self.cube_floating_base = self._require(self.cube_body.floatingbase, "pick_cube floating base") + self.cube_geom = self._require_index(self.model.get_geom_index("pick_cube_geom"), "geom pick_cube_geom") + self.finger_geoms = [ + self._finger_collision_geom("gripper_Left_Support_Link"), + self._finger_collision_geom("gripper_Right_Support_Link"), + ] + self.target_body = self._require(self.model.get_body("place_target"), "body place_target") + self.joints = [self._require(self.model.get_joint(f"joint_{index}"), f"joint_{index}") for index in range(1, 7)] + self.gripper_joint = self._require(self.model.get_joint("gripper_Left_1_Joint"), "joint gripper_Left_1_Joint") + self.actuators = [self._require(self.model.get_actuator(index), f"actuator {index}") for index in range(7)] + ctrl_ranges = np.asarray(self.model.actuator_ctrl_limits, dtype=float) + if ctrl_ranges.shape == (2, self.model.num_actuators): + ctrl_ranges = ctrl_ranges.T + if ctrl_ranges.shape != (self.model.num_actuators, 2): + raise ValueError(f"MotrixSim actuator control limits have an unexpected shape: {ctrl_ranges.shape}") + self.ctrl_ranges = ctrl_ranges.copy() + self.home_keyframe = next((item for item in self.model.keyframes if item.name == "home"), self.model.keyframes[0] if self.model.keyframes else None) + self._reset_home() + self.target_home = np.asarray(self.target_body.get_position(self.data), dtype=float).copy() + offset = [*np.asarray(self.gripper_site.local_pos, dtype=float), *np.asarray(self.gripper_site.local_quat, dtype=float)] + self.ik_chain = ik.IkChain( + self.model, + end_link=self.gripper_site.parent_link.name, + start_link="base_link", + end_effector_offset=offset, + ) + self.ik_solver = ik.DlsSolver(max_iter=200, step_size=0.4, tolerance=1e-3, damping=4e-3) + camera_index = self.model.cameras.get_index("overview") + self.model.cameras[camera_index].set_render_target("image", width, height) + self._render_context = RenderApp(headless=True) + self.renderer = self._render_context.__enter__() + self.renderer.launch(self.model) + self.render_camera = self.renderer.get_camera(camera_index) + self.renderer.sync(self.data, wait=True) + self.simulation_time = 0.0 - def _id(self, kind: Any, name: str) -> int: - value = self.mujoco.mj_name2id(self.model, kind, name) - if value < 0: - raise ValueError(f"required RM65 scene object is missing: {name}") + @staticmethod + def _require(value: Any, description: str) -> Any: + if value is None: + raise ValueError(f"required RM65 scene object is missing: {description}") + return value + + @staticmethod + def _require_index(value: Any, description: str) -> int: + if value is None: + raise ValueError(f"required RM65 scene object is missing: {description}") return int(value) - def close(self) -> None: - self.renderer.close() + def _finger_collision_geom(self, link_name: str) -> int: + candidates = [ + geom + for geom in self.model.geoms + if getattr(geom.link, "name", None) == link_name and int(geom.collision_group) != 0 and int(geom.collision_affinity) != 0 + ] + if len(candidates) != 1: + raise ValueError(f"expected one collidable finger geom on {link_name}, received {len(candidates)}") + return int(candidates[0].index) - def run_episode(self, *, max_steps: int) -> EpisodeResult: - mujoco = self.mujoco - mujoco.mj_resetDataKeyframe(self.model, self.data, 0) - cube_home = self.data.qpos[self.cube_qadr : self.cube_qadr + 3].astype(float).copy() + def _reset_home(self) -> None: + self.data.reset(self.model) + if self.home_keyframe is not None: + self.home_keyframe.apply(self.data) + self.model.forward_kinematic(self.data) + + def close(self) -> None: + self._render_context.__exit__(None, None, None) + + def reset_randomized(self) -> tuple[np.ndarray, np.ndarray, float]: + self._reset_home() + cube_home = np.asarray(self.cube_floating_base.get_translation(self.data), dtype=float).copy() cube = cube_home + np.array([self.rng.uniform(-0.010, 0.010), self.rng.uniform(-0.010, 0.010), 0.0], dtype=float) + self.cube_floating_base.set_translation(self.data, cube) + self.cube_floating_base.set_rotation(self.data, np.array([0.0, 0.0, 0.0, 1.0], dtype=float)) + self.cube_floating_base.set_global_linear_velocity(self.data, np.zeros(3, dtype=float)) + self.cube_floating_base.set_global_angular_velocity(self.data, np.zeros(3, dtype=float)) target = np.array( [ self.target_home[0] + self.rng.uniform(-0.025, 0.025), @@ -81,26 +132,69 @@ class RM65PickPlaceRuntime: ], dtype=float, ) - self.data.qpos[self.cube_qadr : self.cube_qadr + 3] = cube - self.data.qpos[self.cube_qadr + 3 : self.cube_qadr + 7] = np.array([1.0, 0.0, 0.0, 0.0]) - self.data.qvel[self.model.jnt_dofadr[self.cube_joint] : self.model.jnt_dofadr[self.cube_joint] + 6] = 0 - self.model.body_pos[self.target_body] = np.array([target[0], target[1], self.target_home[2]]) - mujoco.mj_forward(self.model, self.data) - for _ in range(120): - mujoco.mj_step(self.model, self.data) - settled_z = float(self.data.qpos[self.cube_qadr + 2]) + if self.target_body.mocap is not None: + self.target_body.mocap.set_pose( + self.data, + np.array([target[0], target[1], self.target_home[2], 0.0, 0.0, 0.0, 1.0], dtype=float), + ) + else: + target[:2] = self.target_home[:2] + self.model.forward_kinematic(self.data) + self.model.step_n(self.data, 120) + self.simulation_time = 120 * float(self.model.options.timestep) + self.renderer.sync(self.data, wait=True) + return cube, target, float(self.cube_position()[2]) + + def capture_frame(self) -> np.ndarray: + capture = self.render_camera.capture() + self.renderer.sync(self.data, wait=True) + image = capture.take_image() + if image is None: + raise RuntimeError("MotrixSim overview camera did not return an RGB frame") + pixels = np.asarray(image.pixels, dtype=np.uint8) + if pixels.ndim != 3 or pixels.shape[2] < 3: + raise RuntimeError(f"MotrixSim overview camera returned invalid pixels: {pixels.shape}") + return np.ascontiguousarray(pixels[:, :, :3]).copy() + + def joint_state(self) -> np.ndarray: + values = [float(joint.get_dof_pos(self.data)[0]) for joint in self.joints] + values.append(float(self.gripper_joint.get_dof_pos(self.data)[0])) + return np.asarray(values, dtype=np.float32) + + def clip_action(self, action: np.ndarray) -> np.ndarray: + action = np.asarray(action, dtype=np.float32).copy() + for index in range(7): + action[index] = np.clip(action[index], self.ctrl_ranges[index, 0], self.ctrl_ranges[index, 1]) + return action + + def apply_action(self, action: np.ndarray, *, hold_steps: int = CONTROL_HOLD_STEPS) -> None: + action = self.clip_action(action) + for actuator, value in zip(self.actuators, action): + actuator.set_ctrl(self.data, float(value)) + self.model.step_n(self.data, hold_steps) + self.simulation_time += hold_steps * float(self.model.options.timestep) + + def cube_position(self) -> np.ndarray: + return np.asarray(self.cube_body.get_position(self.data), dtype=float).copy() + + def gripper_position(self) -> np.ndarray: + return np.asarray(self.gripper_site.get_position(self.data), dtype=float).copy() + + def run_episode(self, *, max_steps: int) -> EpisodeResult: + cube, target, settled_z = self.reset_randomized() # The named site is about 3 cm above the cube center at bilateral pad contact. grasp_site = np.array([cube[0] - 0.014, cube[1] - 0.020, settled_z + 0.020]) # Contact-calibrated negative-wrist branch for the packaged RM65 gripper. # This seed is a robot calibration pose, not an object-pose override. seed_q = np.array([-1.2468, -0.1011, 1.8666, -1.9230, -1.5982, 1.5636], dtype=float) - q_backup = self.data.qpos.copy() - self.data.qpos[self.qpos_adrs] = seed_q - mujoco.mj_forward(self.model, self.data) - grasp_orientation = self.data.site_xmat[self.site_id].reshape(3, 3).copy() - self.data.qpos[:] = q_backup - mujoco.mj_forward(self.model, self.data) + q_backup = np.asarray(self.data.dof_pos, dtype=float).copy() + for joint, value in zip(self.joints, seed_q): + joint.set_dof_pos(self.data, np.array([value], dtype=float)) + self.model.forward_kinematic(self.data) + grasp_orientation = np.asarray(self.gripper_site.get_pose(self.data), dtype=float)[3:7].copy() + self.data.set_dof_pos(q_backup, self.model) + self.model.forward_kinematic(self.data) phases = [ ("approach", grasp_site + np.array([0.0, 0.0, 0.20]), 0.0, 24), @@ -124,24 +218,20 @@ class RM65PickPlaceRuntime: for phase, position, gripper, nominal_steps in phases: target_q = self.solve_ik_6d(position, grasp_orientation, seed_q=seed_q) phase_steps = max(2, int(round(nominal_steps * scale))) - start_ctrl = self.data.ctrl[:6].copy() + start_ctrl = np.asarray(self.data.actuator_ctrls[:6], dtype=float).copy() for local_step in range(phase_steps): alpha = _smoothstep((local_step + 1) / phase_steps) action = np.empty(7, dtype=np.float32) action[:6] = start_ctrl * (1 - alpha) + target_q * alpha action[6] = gripper - self.renderer.update_scene(self.data, camera="overview") - frame = self.renderer.render().copy() - observation = np.concatenate([self.data.qpos[self.qpos_adrs], [self.data.qpos[self.gripper_qadr]]]).astype(np.float32) - cube_before = self.data.qpos[self.cube_qadr : self.cube_qadr + 3].astype(float).copy() - self.data.ctrl[:7] = action - # The RM65 model has conservative actuator force limits; each policy - # command is held long enough for the physical joints to settle. - for _ in range(CONTROL_HOLD_STEPS): - mujoco.mj_step(self.model, self.data) + frame = self.capture_frame() + observation = self.joint_state() + cube_before = self.cube_position() + self.apply_action(action) touching = self.finger_contacts() bilateral_contact_steps += int(len(touching) == 2) - max_cube_z = max(max_cube_z, float(self.data.qpos[self.cube_qadr + 2])) + cube_after = self.cube_position() + max_cube_z = max(max_cube_z, float(cube_after[2])) observations.append(observation) actions.append(action.copy()) contacts.append(np.array([int(geom in touching) for geom in sorted(self.finger_geoms)], dtype=np.uint8)) @@ -149,22 +239,22 @@ class RM65PickPlaceRuntime: trajectory.append( { "step": step_index, - "time": float(self.data.time), + "time": self.simulation_time, "phase": phase, "qpos": observation.tolist(), "action": action.tolist(), "action_alignment": ACTION_ALIGNMENT, "cube_pos_before": cube_before.tolist(), - "cube_pos": self.data.qpos[self.cube_qadr : self.cube_qadr + 3].astype(float).tolist(), - "gripper_pos": self.data.site_xpos[self.site_id].astype(float).tolist(), + "cube_pos": cube_after.tolist(), + "gripper_pos": self.gripper_position().tolist(), "bilateral_contact": len(touching) == 2, "touching_fingers": sorted(int(item) for item in touching), - "finger_positions": [self.data.geom_xpos[item].astype(float).tolist() for item in sorted(self.finger_geoms)], + "finger_positions": [np.asarray(site.get_position(self.data), dtype=float).tolist() for site in self.finger_sites], } ) step_index += 1 - final_cube = self.data.qpos[self.cube_qadr : self.cube_qadr + 3].copy() + final_cube = self.cube_position() lift_height = float(max_cube_z - settled_z) place_error = float(np.linalg.norm(final_cube - target)) success = bilateral_contact_steps > 0 and lift_height >= 0.10 and place_error <= 0.05 @@ -183,53 +273,21 @@ class RM65PickPlaceRuntime: ) def finger_contacts(self) -> set[int]: - touching: set[int] = set() - for index in range(self.data.ncon): - contact = self.data.contact[index] - pair = {int(contact.geom1), int(contact.geom2)} - if self.cube_geom in pair: - touching.update(pair & self.finger_geoms) - return touching + pairs = np.asarray([[self.cube_geom, geom] for geom in self.finger_geoms], dtype=np.uint32) + active = np.asarray(self.model.get_contact_query(self.data).is_colliding(pairs), dtype=bool) + return {geom for geom, colliding in zip(self.finger_geoms, active) if bool(colliding)} - def solve_position_ik(self, target_position: np.ndarray) -> np.ndarray: - q_backup = self.data.qpos.copy() - q = self.data.qpos[self.qpos_adrs].copy() - for _ in range(120): - self.data.qpos[self.qpos_adrs] = q - self.mujoco.mj_forward(self.model, self.data) - error = target_position - self.data.site_xpos[self.site_id] - if np.linalg.norm(error) < 0.004: - break - jacp = np.zeros((3, self.model.nv)) - jacr = np.zeros((3, self.model.nv)) - self.mujoco.mj_jacSite(self.model, self.data, jacp, jacr, self.site_id) - jacobian = jacp[:, self.dof_adrs] - q += np.clip(_damped_least_squares(jacobian, error, 2e-3), -0.06, 0.06) - q = self._clip_arm(q) - self.data.qpos[:] = q_backup - self.mujoco.mj_forward(self.model, self.data) - return q - - def solve_ik_6d(self, target_position: np.ndarray, target_rotation: np.ndarray, *, seed_q: np.ndarray | None = None) -> np.ndarray: - q_backup = self.data.qpos.copy() - q = seed_q.copy() if seed_q is not None else self.data.qpos[self.qpos_adrs].copy() - for _ in range(160): - self.data.qpos[self.qpos_adrs] = q - self.mujoco.mj_forward(self.model, self.data) - position_error = target_position - self.data.site_xpos[self.site_id] - current_rotation = self.data.site_xmat[self.site_id].reshape(3, 3) - rotation_error = _rotation_vector(target_rotation @ current_rotation.T) - error = np.concatenate([position_error, 0.45 * rotation_error]) - if np.linalg.norm(position_error) < 0.005 and np.linalg.norm(rotation_error) < 0.04: - break - jacp = np.zeros((3, self.model.nv)) - jacr = np.zeros((3, self.model.nv)) - self.mujoco.mj_jacSite(self.model, self.data, jacp, jacr, self.site_id) - jacobian = np.vstack([jacp[:, self.dof_adrs], 0.45 * jacr[:, self.dof_adrs]]) - q += np.clip(_damped_least_squares(jacobian, error, 4e-3), -0.05, 0.05) - q = self._clip_arm(q) - self.data.qpos[:] = q_backup - self.mujoco.mj_forward(self.model, self.data) + def solve_ik_6d(self, target_position: np.ndarray, target_quaternion: np.ndarray, *, seed_q: np.ndarray | None = None) -> np.ndarray: + q_backup = np.asarray(self.data.dof_pos, dtype=float).copy() + if seed_q is not None: + for joint, value in zip(self.joints, seed_q): + joint.set_dof_pos(self.data, np.array([value], dtype=float)) + self.model.forward_kinematic(self.data) + target_pose = np.concatenate([np.asarray(target_position, dtype=float), np.asarray(target_quaternion, dtype=float)]) + solution = np.asarray(self.ik_solver.solve(self.ik_chain, self.data, target_pose), dtype=float) + q = self._clip_arm(solution[2:8]) + self.data.set_dof_pos(q_backup, self.model) + self.model.forward_kinematic(self.data) return q def _clip_arm(self, q: np.ndarray) -> np.ndarray: @@ -268,7 +326,7 @@ def collect(scene_path: Path, out_dir: Path, *, required_successes: int, max_att manifest = { "schema": "real2sim.discoverse_act_dataset.v1", "controller": "rm65_pick_place_6d_ik", - "runtime": "mujoco", + "runtime": "motrixsim", "scene": str(scene_path), "camera_names": ["overview"], "image_encoding": {"codec": "jpeg", "semantic_color_order": "RGB", "opencv_decode_color_order": "BGR"}, @@ -388,7 +446,7 @@ def _smoothstep(value: float) -> float: def main() -> int: - parser = argparse.ArgumentParser(description="Collect physical RM65 pick-place episodes for DISCOVERSE ACT") + parser = argparse.ArgumentParser(description="Collect physical RM65 pick-place episodes in MotrixSim for DISCOVERSE ACT") parser.add_argument("--scene", type=Path, required=True) parser.add_argument("--out", type=Path, required=True) parser.add_argument("--required-successes", type=int, default=50) diff --git a/src/real2sim_platform/runtime_gate.py b/src/real2sim_platform/runtime_gate.py index a86f6be..3fe9f59 100644 --- a/src/real2sim_platform/runtime_gate.py +++ b/src/real2sim_platform/runtime_gate.py @@ -214,11 +214,26 @@ def _simulator_probe() -> dict[str, Any]: scene_path = Path(temporary) / "scene.xml" scene_path.write_text(xml, encoding="utf-8") from motrixsim import SceneData, load_model, step + from motrixsim.render import RenderApp motrix_model = load_model(str(scene_path)) motrix_data = SceneData(motrix_model) - for _ in range(8): - step(motrix_model, motrix_data) + motrix_model.cameras[0].set_render_target("image", 160, 120) + with RenderApp(headless=True) as motrix_renderer: + motrix_renderer.launch(motrix_model) + for _ in range(8): + step(motrix_model, motrix_data) + capture = motrix_renderer.get_camera(0).capture() + motrix_renderer.sync(motrix_data, wait=True) + motrix_image = capture.take_image() + if motrix_image is None: + raise RuntimeError("MotrixSim headless renderer did not return an image") + motrix_pixels = np.asarray(motrix_image.pixels, dtype=np.uint8)[:, :, :3] + motrix_mean = float(motrix_pixels.mean()) + motrix_std = float(motrix_pixels.std()) + motrix_nonzero = int(np.count_nonzero(motrix_pixels)) + if motrix_nonzero <= 0 or motrix_std <= 0: + raise RuntimeError(f"MotrixSim headless renderer produced a blank image: mean={motrix_mean}, std={motrix_std}") ffmpeg = os.environ.get("REAL2SIM_FFMPEG_BIN", "").strip() or shutil.which("ffmpeg") if not ffmpeg: raise RuntimeError("ffmpeg executable is unavailable") @@ -258,7 +273,8 @@ def _simulator_probe() -> dict[str, Any]: "ok": True, "mujoco_time": float(data.time), "motrix_timestep": float(motrix_model.options.timestep), - "renderer": {"shape": list(image.shape), "mean": image_mean, "std": image_std, "nonzero_pixels": nonzero_pixels}, + "mujoco_renderer": {"shape": list(image.shape), "mean": image_mean, "std": image_std, "nonzero_pixels": nonzero_pixels}, + "motrixsim_renderer": {"shape": list(motrix_pixels.shape), "mean": motrix_mean, "std": motrix_std, "nonzero_pixels": motrix_nonzero}, "ffmpeg": {"path": ffmpeg, "video_size": video_size, "codec": "libx264"}, } diff --git a/src/real2sim_platform/server.py b/src/real2sim_platform/server.py index 73be769..600d46d 100644 --- a/src/real2sim_platform/server.py +++ b/src/real2sim_platform/server.py @@ -1548,7 +1548,7 @@ def _capabilities(cfg: PlatformConfig, storage: BosStorage, store: PlatformStore "mujoco": mujoco_ready, "motrixsim": motrixsim_ready, "discoverse_act": cfg.discoverse_root.is_dir() and act_root.is_dir() and discoverse_commit == cfg.discoverse_commit and act_commit == cfg.act_commit and torch_ready, - "rm65_controller": rm65_controller.is_file() and mujoco_ready, + "rm65_controller": rm65_controller.is_file() and motrixsim_ready, } worker_status: dict[str, Any] = {"connected": cfg.worker_mode != "api", "age_seconds": 0 if cfg.worker_mode != "api" else None} if store is not None and cfg.worker_mode == "api": @@ -1566,7 +1566,7 @@ def _capabilities(cfg: PlatformConfig, storage: BosStorage, store: PlatformStore mujoco=bool(checks.get("mujoco_python")), motrixsim=bool(checks.get("motrixsim_python")), discoverse_act=all(bool(checks.get(name)) for name in ("discoverse_act", "torch_cuda", "act_import")), - rm65_controller=bool(checks.get("rm65_controller")) and bool(checks.get("mujoco_python")), + rm65_controller=bool(checks.get("rm65_controller")) and bool(checks.get("motrixsim_python")), ) else: for name in local_runtime: diff --git a/src/real2sim_platform/static/index.html b/src/real2sim_platform/static/index.html index 5084ab9..67cd07a 100644 --- a/src/real2sim_platform/static/index.html +++ b/src/real2sim_platform/static/index.html @@ -496,7 +496,7 @@

数据采集

-

选择已重建场景和专家策略,批量生成可训练的 rollout 数据。

+

在 MotrixSim 中运行真实物理、6D IK、双指接触与 RGB 采集,生成可训练的 rollout 数据。

diff --git a/tests/test_production_platform.py b/tests/test_production_platform.py index 8f00899..97715e9 100644 --- a/tests/test_production_platform.py +++ b/tests/test_production_platform.py @@ -251,10 +251,7 @@ def test_scene_composition_catalog_and_payload_are_registered(tmp_path: Path) -> assert queued.json()["task"]["type"] == "scene_composition" -@pytest.mark.integration -def test_real_rm65_scene_composition_loads_and_steps_in_both_runtimes(tmp_path: Path) -> None: - mujoco = pytest.importorskip("mujoco") - pytest.importorskip("motrixsim") +def _build_real_rm65_composition_package(package: Path) -> tuple[Path, dict]: candidates = [ Path("/opt/motrixsim-docs/examples/assets/rm65_open_cabinet/mjx_rm65.xml"), Path("/Users/d-robotics/workSpace/motrixsim-docs/examples/assets/rm65_open_cabinet/mjx_rm65.xml"), @@ -262,7 +259,6 @@ def test_real_rm65_scene_composition_loads_and_steps_in_both_runtimes(tmp_path: robot_source = next((path for path in candidates if path.is_file()), None) if robot_source is None: pytest.skip("real MotrixSim RM65 asset checkout is unavailable") - package = tmp_path / "composition" runtime = package / "simulator_assets" / "scene.xml" (package / "assets" / "meshes").mkdir(parents=True) runtime.parent.mkdir(parents=True) @@ -289,6 +285,14 @@ def test_real_rm65_scene_composition_loads_and_steps_in_both_runtimes(tmp_path: source_paths={}, validation_steps=120, ) + return runtime, result + + +@pytest.mark.integration +def test_real_rm65_scene_composition_loads_and_steps_in_both_runtimes(tmp_path: Path) -> None: + mujoco = pytest.importorskip("mujoco") + motrixsim = pytest.importorskip("motrixsim") + runtime, result = _build_real_rm65_composition_package(tmp_path / "composition") assert result["validation"]["mujoco"]["status"] == "loaded_and_stepped" assert result["validation"]["motrixsim"]["status"] == "loaded_and_stepped" assert result["collection_ready"] is True @@ -303,6 +307,8 @@ def test_real_rm65_scene_composition_loads_and_steps_in_both_runtimes(tmp_path: (mujoco.mjtObj.mjOBJ_CAMERA, "overview"), ): assert mujoco.mj_name2id(model, object_type, name) >= 0 + motrix_model = motrixsim.load_model(str(runtime)) + assert motrix_model.get_body("place_target").mocap is not None assert Path(result["composition"]["robot"]["path"]).stat().st_size > 1_000_000 @@ -795,10 +801,10 @@ def test_motrixsim_runtime_scene_uses_current_python_when_module_is_installed(mo @pytest.mark.integration def test_real_rm65_physical_collection_writes_act_hdf5(tmp_path: Path) -> None: - scene = ROOT / "workspace/motrixsim_assets/parlour_rm65_pick_place/simulator_assets/rm65_table_pick_place.xml" - if not scene.is_file(): - pytest.skip("real imported RM65 scene asset is not present") + pytest.importorskip("motrixsim") + scene, _ = _build_real_rm65_composition_package(tmp_path / "composition") manifest = collect(scene, tmp_path / "collection", required_successes=1, max_attempts=2, max_steps=240, seed=7, width=160, height=120) + assert manifest["runtime"] == "motrixsim" assert manifest["successful_episodes"] == 1 assert manifest["episodes"][0]["lift_height"] >= 0.10 assert manifest["episodes"][0]["place_error"] <= 0.05