diff --git a/movensys_sample/movensys_robopoly/pick_and_place.py b/movensys_sample/movensys_robopoly/pick_and_place.py index 4c655e1..3602392 100644 --- a/movensys_sample/movensys_robopoly/pick_and_place.py +++ b/movensys_sample/movensys_robopoly/pick_and_place.py @@ -11,48 +11,48 @@ board_positions = { "GO": { "red_cube": { - "pos": [-0.38632, -0.14548, 0.3], + "pos": [-0.38632, -0.153, 0.3], "ori": [3.14, 0.0, -1.57], "sim_pos": [-0.37876, -0.15907, 0.3] }, "green_cube": { - "pos": [-0.32625, -0.14554, 0.3], + "pos": [-0.32625, -0.153, 0.3], "ori": [3.14, 0.0, -1.57], "sim_pos": [-0.3323, -0.15907, 0.3] } }, "BOSTON": { "red_cube": { - "pos": [-0.38640, -0.09043, 0.3], + "pos": [-0.38640, -0.09308, 0.3], "ori": [3.14, 0.0, -1.57], "sim_pos": [-0.38615, -0.09402,0.3] }, "green_cube": { - "pos": [-0.32625, -0.09051, 0.3], + "pos": [-0.32625, -0.09308, 0.3], "ori": [3.14, 0.0, -1.57], "sim_pos": [-0.34041, -0.09396, 0.3] } }, "SEOUL": { "red_cube": { - "pos": [-0.38640, -0.01535, 0.3], + "pos": [-0.38640, -0.023, 0.3], "ori": [3.14, 0.0, -1.57], "sim_pos": [-0.38615, -0.02562, 0.3] }, "green_cube": { - "pos": [-0.32625, -0.01535, 0.3], + "pos": [-0.32625, -0.023, 0.3], "ori": [3.14, 0.0, -1.57], "sim_pos": [-0.34041, -0.02614, 0.3] } }, "DESERT_ISLAND": { "red_cube": { - "pos": [-0.38640, 0.04475, 0.3], + "pos": [-0.38640, 0.04465, 0.3], "ori": [3.14, 0.0, -1.57], "sim_pos": [-0.38615, 0.0405, 0.3] }, "green_cube": { - "pos": [-0.32625, 0.04475, 0.3], + "pos": [-0.32625, 0.04465, 0.3], "ori": [3.14, 0.0, -1.57], "sim_pos": [-0.34041, 0.04039, 0.3] } @@ -124,55 +124,55 @@ "sim_pos": [0.07915, -0.09243, 0.3] }, "green_cube": { - "pos": [0.11401, -0.09306, 0.3], + "pos": [0.11396, -0.09308, 0.3], "ori": [3.14, 0.0, -1.57], "sim_pos": [0.12049, -0.09362, 0.3] } }, "GO_TO_DESERT_ISLAND": { "red_cube": { - "pos": [0.0538, -0.15813, 0.3], + "pos": [0.0538, -0.153, 0.3], "ori": [3.14, 0.0, -1.57], "sim_pos": [0.07991, -0.16428, 0.3] }, "green_cube": { - "pos": [0.10897, -0.16310, 0.3], + "pos": [0.10895, -0.153, 0.3], "ori": [3.14, 0.0, -1.57], "sim_pos": [0.12049, -0.16341, 0.3] } }, "NEW_YORK": { "red_cube": { - "pos": [-0.05662, -0.16319, 0.3], + "pos": [-0.05662, -0.153, 0.3], "ori": [3.14, 0.0, -1.57], "sim_pos": [-0.03824, -0.16428, 0.3] }, "green_cube": { - "pos": [0.0352, -0.16317, 0.3], + "pos": [0.01015, -0.153, 0.3], "ori": [3.14, 0.0, -1.57], "sim_pos": [0.00573, -0.16341, 0.3] } }, "CHANCE": { "red_cube": { - "pos": [-0.16186, -0.16317, 0.3], + "pos": [-0.16186, -0.153, 0.3], "ori": [3.14, 0.0, -1.57], "sim_pos": [-0.1527, -0.16428, 0.3] }, "green_cube": { - "pos": [-0.10666, -0.16320, 0.3], + "pos": [-0.10666, -0.153, 0.3], "ori": [3.14, 0.0, -1.57], "sim_pos": [-0.11251, -0.16341, 0.3] } }, "LONDON": { "red_cube": { - "pos": [-0.27194, -0.16313, 0.30], + "pos": [-0.27194, -0.153, 0.30], "ori": [3.14, 0.0, -1.57], "sim_pos": [-0.26724, -0.16428, 0.3] }, "green_cube": { - "pos": [-0.21182, -0.16319, 0.3], + "pos": [-0.21182, -0.153, 0.3], "ori": [3.14, 0.0, -1.57], "sim_pos": [-0.22484, -0.16341, 0.3] } @@ -273,6 +273,43 @@ def get_eef_pose(): def set_scales(vel, acc): return _post("/api/config/scales", {"vel_scale": vel, "acc_scale": acc}) + +# Isaac-Sim spawn z. Mirrors `z_target_pose_spawn` in +# apriltag_pick_and_place.cpp's yaml (0.07 = table-relative). EEF z would +# be at gripper height — too high for the object's actual rest pose. +_ISAAC_SPAWN_Z = 0.07 + + +def _publish_isaac_target_pose(target_object: str, z: float = _ISAAC_SPAWN_Z) -> None: + """Teleport the Isaac-Sim counterpart of `target_object` to where the + real arm is, so the simulated object stays in lockstep through the + pickup. Matches apriltag_pick_and_place.cpp's target_spawn block — + one POST per pickup, fired just before the gripper closes. No-op in + DRY_RUN; logs and continues on HTTP errors so a misconfigured Isaac + never breaks a real-world pick.""" + if DRY_RUN: + return + try: + resp = requests.post( + f"{URL}/api/isaac/spawn_target", + json={"target": target_object, "z": z}, + timeout=5.0, + ) + if resp.ok: + body = resp.json() + logger.info( + "[isaac] spawn_target %s -> %s @ %s", + target_object, body.get("topic"), + body.get("pose", {}).get("position"), + ) + else: + logger.warning( + "[isaac] spawn_target %s failed: %s %s", + target_object, resp.status_code, resp.text[:200], + ) + except Exception as exc: + logger.warning("[isaac] spawn_target %s error: %s", target_object, exc) + class PnP: _BIN_CENTERS = (0.0, -math.pi / 2, -math.pi, math.pi / 2) @@ -550,6 +587,12 @@ def pick_and_place(self, board_pos: str = "GO"): self._toward_target(self.target_object, target_pos, target_ori) + # Sync Isaac Sim: teleport the simulated counterpart of this object + # to the current EEF pose (axis-swapped on the orchestrator side) so + # a simulated arm grabs it in lockstep with the real one. Mirrors + # apriltag_pick_and_place.cpp's `target_spawn` step. + _publish_isaac_target_pose(self.target_object) + # grasp gripper(close=True) _sleep(_GRIPPER_SETTLE_S) diff --git a/movensys_sample/movensys_robopoly/static/app.css b/movensys_sample/movensys_robopoly/static/app.css index 672ce6e..5345318 100644 --- a/movensys_sample/movensys_robopoly/static/app.css +++ b/movensys_sample/movensys_robopoly/static/app.css @@ -312,6 +312,7 @@ button:disabled { opacity: 0.4; cursor: not-allowed; } object-fit: contain; border-radius: 4px; background: #000; + transform: rotate(180deg); } .board-coords { font-family: "JetBrains Mono", ui-monospace, monospace; diff --git a/movensys_vlm/doc/running.md b/movensys_vlm/doc/running.md index 2a13fdb..840dd6e 100644 --- a/movensys_vlm/doc/running.md +++ b/movensys_vlm/doc/running.md @@ -81,7 +81,8 @@ COMPOSE_PROFILES=$XPU_CORE docker compose -f movensys_vlm.yaml up -d ```bash cd ~/workspaces/movensys-intelligence/movensys_vlm/docker COMPOSE_PROFILES=$XPU_CORE docker compose -f whisper.yaml down -WHISPER_DEFAULT_LANGUAGE=en COMPOSE_PROFILES=$XPU_CORE docker compose -f whisper.yaml up -d --force-recreate +WHISPER_DEFAULT_LANGUAGE=en COMPOSE_PROFILES=$XPU_CORE docker compose -f whisper.yaml build +WHISPER_DEFAULT_LANGUAGE=en COMPOSE_PROFILES=$XPU_CORE docker compose -f whisper.yaml up -d ``` diff --git a/movensys_vlm/ros2_node.py b/movensys_vlm/ros2_node.py index b38f79b..3fd679b 100644 --- a/movensys_vlm/ros2_node.py +++ b/movensys_vlm/ros2_node.py @@ -136,6 +136,16 @@ def __init__(self): self.create_subscription(sensor_msgs.msg.Image, "/yolo_dice_detector/debug_image", self._cb_yolo_dice_debug, 1, callback_group=cb) self.create_subscription(sensor_msgs.msg.Image, "/yolo_cube_detector/debug_image", self._cb_yolo_cube_debug, 1, callback_group=cb) + # Isaac Sim object-teleport publishers. Robopoly's pick_and_place + # subprocess hits /api/isaac/spawn_target during a pickup so the + # simulated counterpart of the dice / cube lands under the + # simulated gripper. Mirrors apriltag_pick_and_place.cpp's + # `target_spawn` block — one publisher per object. + self._isaac_pubs = { + target: self.create_publisher(geometry_msgs.msg.Pose, topic, 10) + for target, topic in self.ISAAC_TARGETS.items() + } + self.cli_get_eef_pose = self.create_client(GetEefPose, "/wmx/moveit2/get_eef_pose", callback_group=cb) self.cli_gripper = self.create_client(std_srvs.srv.SetBool, "/wmx/set_gripper", callback_group=cb) self.cli_abs_base_cart = self.create_client(MovePose, "/wmx/moveit2/absolute_base_eef_cartesian", callback_group=cb) @@ -237,6 +247,36 @@ def _cb_yolo_cube_debug(self, msg: sensor_msgs.msg.Image): _TF_CHILD = "camera_top_color_optical_frame" _YOLO_FRAMES = {"yolo_cube_red", "yolo_cube_green", "dice"} + # Maps the pick_and_place.py target name to the Isaac Sim teleport topic. + ISAAC_TARGETS = { + "dice": "/dice_pose_sub", + "red_cube": "/red_pose_sub", + "green_cube": "/green_pose_sub", + } + + def publish_isaac_target_pose(self, target: str, pose: dict) -> str: + """Publish a `geometry_msgs/Pose` to the per-target Isaac-Sim topic + so the simulated dice / cube teleports to `pose`. Returns the topic + name. Mirrors `apriltag_pick_and_place.cpp` (target_spawn block). + """ + pub = self._isaac_pubs.get(target) + if pub is None: + raise ValueError( + f"unknown target {target!r}; expected one of {list(self.ISAAC_TARGETS)}" + ) + msg = geometry_msgs.msg.Pose() + p = pose["position"] + o = pose["orientation"] + msg.position.x = float(p["x"]) + msg.position.y = float(p["y"]) + msg.position.z = float(p["z"]) + msg.orientation.x = float(o["x"]) + msg.orientation.y = float(o["y"]) + msg.orientation.z = float(o["z"]) + msg.orientation.w = float(o["w"]) + pub.publish(msg) + return self.ISAAC_TARGETS[target] + def _cb_tf_static(self, msg: tf2_msgs.msg.TFMessage): for t in msg.transforms: if t.header.frame_id != self._TF_PARENT or t.child_frame_id != self._TF_CHILD: diff --git a/movensys_vlm/router.py b/movensys_vlm/router.py index ca9c30a..19fc48d 100644 --- a/movensys_vlm/router.py +++ b/movensys_vlm/router.py @@ -365,6 +365,63 @@ def set_scales(body: ScalesRequest): return rn.set_scales(body.vel_scale, body.acc_scale) +# --------------------------------------------------------------------------- +# Isaac Sim — object teleport sync +# --------------------------------------------------------------------------- + +class IsaacSpawnTargetRequest(BaseModel): + # "dice", "red_cube", or "green_cube" — maps to /{dice,red,green}_pose_sub. + target: str + # Optional explicit pose. When omitted, the orchestrator falls back to + # the latest EEF pose with the apriltag axis swap + # (x_iso = -y_base, y_iso = x_base) so the Isaac frame matches the + # manipulator base frame. + pose: Optional[dict] = None + # Override z when the pose is derived from EEF. EEF z is the gripper + # height (well above the table during a pick), so a fixed table-relative + # z gives a more useful spawn point. Mirrors `z_target_pose_spawn` in + # apriltag_pick_and_place.cpp (yaml default 0.07). + z: Optional[float] = None + + model_config = { + "json_schema_extra": { + "example": {"target": "dice", "z": 0.07} + } + } + + +@router.post("/api/isaac/spawn_target") +def isaac_spawn_target(body: IsaacSpawnTargetRequest): + """Publish a Pose to /{dice,red,green}_pose_sub so Isaac Sim teleports + the matching object. Called by movensys_robopoly's `pick_and_place.py` + immediately before the gripper closes on a pickup, so the simulated + counterpart of the dice / cube ends up under the simulated gripper — + same pattern as `apriltag_pick_and_place.cpp`'s target_spawn block. + """ + if rn.ros_node is None: + raise HTTPException(503, detail="ROS node not running") + if body.pose is not None: + pose = body.pose + else: + eef = rn.ros_node.latest_eef_pose + if eef is None: + raise HTTPException(503, detail="No EEF pose received yet") + ep, eo = eef["position"], eef["orientation"] + pose = { + "position": { + "x": -float(ep["y"]), + "y": float(ep["x"]), + "z": float(body.z) if body.z is not None else float(ep["z"]), + }, + "orientation": dict(eo), + } + try: + topic = rn.ros_node.publish_isaac_target_pose(body.target, pose) + except ValueError as exc: + raise HTTPException(400, detail=str(exc)) + return {"target": body.target, "topic": topic, "pose": pose} + + # --------------------------------------------------------------------------- # VLM inference # ---------------------------------------------------------------------------