Feature/spawn dice and pieces isaacsim

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)

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;

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

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:

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