feat: run RM65 collection in MotrixSim

This commit is contained in:
eust-w 2026-07-12 17:38:24 +08:00
parent 5923049af6
commit 08598438c0
8 changed files with 229 additions and 161 deletions

View File

@ -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)

View File

@ -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),

View File

@ -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:

View File

@ -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)

View File

@ -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"},
}

View File

@ -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:

View File

@ -496,7 +496,7 @@
<div class="page-head">
<div>
<h1>数据采集</h1>
<p>选择已重建场景和专家策略,批量生成可训练的 rollout 数据。</p>
<p>在 MotrixSim 中运行真实物理、6D IK、双指接触与 RGB 采集,生成可训练的 rollout 数据。</p>
</div>
<button id="createCollectTask" class="primary-button">开始采集</button>
</div>

View File

@ -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