Fix PrismaticJoint (trunk) control: IdealPDActuator with high stiffness

Replace ImplicitActuator (which doesn't work for prismatic joints) with
IdealPDActuator. This bypasses PhysX drive and explicitly computes PD forces
using set_dof_actuation_forces().

Changes:
- stiffness=100k, damping=5k to resist arm reaction forces (~150N) with <2mm deflection
- Read trunk_cmd from ArticulationCfg initial position (single source of truth)
- Integrate trunk diagnostics into robot state output
- Remove hardcoded trunk_target parameter

scripts/environments/teleoperation/xr_teleop/diagnostics.py

@@ -21,9 +21,10 @@ class DiagnosticsReporter:
             jnames = robot.joint_names
             self._left_arm_idx = [i for i, n in enumerate(jnames) if n.startswith("l_joint")]
             self._right_arm_idx = [i for i, n in enumerate(jnames) if n.startswith("r_joint")]
+            self._trunk_idx = jnames.index("PrismaticJoint") if "PrismaticJoint" in jnames else None
 
     def report(self, env, obs, frame: int, last_act: np.ndarray | None = None,
-               xr_client=None, read_gripper_contact=None):
+               xr_client=None, read_gripper_contact=None, trunk_target: float | None = None):
         """Print diagnostics if frame is on interval boundary."""
         if frame % self.interval != 0:
             return
@@ -77,6 +78,12 @@ class DiagnosticsReporter:
             print(f"| Cmd right_arm(abs):         {last_act[12:19]}")
             print(f"| Cmd right_gripper:          {last_act[19:]}  (+1=open -1=close)")
             print(f"| Joystick (x=turn,y=fwd):    {joy_str}")
+            if self._trunk_idx is not None:
+                trunk_pos = robot.data.joint_pos[0, self._trunk_idx].item()
+                trunk_vel = robot.data.joint_vel[0, self._trunk_idx].item()
+                trunk_force = robot.data.applied_torque[0, self._trunk_idx].item() if hasattr(robot.data, 'applied_torque') else float('nan')
+                trunk_tgt_str = f"{trunk_target:.4f}" if trunk_target is not None else "N/A"
+                print(f"| Trunk pos={trunk_pos:.4f}  vel={trunk_vel:.6f}  target={trunk_tgt_str}  force={trunk_force:.2f}")
             print(f"----------------------------------------------------------------")
         else:
             eef_pos = policy_obs["eef_pos"][0].cpu().numpy()

scripts/environments/teleoperation/xr_teleop/dual_arm_head_agent.py

@@ -29,7 +29,6 @@ class DualArmHeadXrAgent(DualArmXrAgent):
                  stream_to: str | None = None,
                  stream_port: int = 12345,
                  stream_bitrate: int = 20_000_000,
-                 trunk_target: float = 0.1,
                  debug_viewports: bool = True):
         super().__init__(
             env, simulation_app,
@@ -40,7 +39,13 @@ class DualArmHeadXrAgent(DualArmXrAgent):
         )
 
         self.head_tracker = HeadTracker()
-        self.trunk_cmd = torch.tensor([trunk_target], dtype=torch.float32)
+
+        # Read trunk initial position from ArticulationCfg (single source of truth)
+        robot = env.unwrapped.scene["robot"]
+        joint_names = robot.data.joint_names
+        trunk_idx = joint_names.index("PrismaticJoint")
+        trunk_init = robot.data.default_joint_pos[0, trunk_idx].item()
+        self.trunk_cmd = torch.tensor([trunk_init], dtype=torch.float32)
 
         # Streaming
         self.streamer: StreamingManager | None = None
@@ -77,6 +82,7 @@ class DualArmHeadXrAgent(DualArmXrAgent):
         self.diagnostics.report(
             self.env, obs, self.sim_frame,
             xr_client=self.shared_client,
+            trunk_target=self.trunk_cmd[0].item(),
         )
 
     def cleanup(self):

source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_2i_cfg.py

@@ -61,11 +61,11 @@ MINDBOT_CFG = ArticulationCfg(
             # NOTE: pos z = 0.44 (base clearance) + PrismaticJoint value.
             # If you change PrismaticJoint, update pos z accordingly to avoid
             # geometry clipping that causes the robot to fall on spawn.
-            "PrismaticJoint": 0.4,
+            "PrismaticJoint": 0.2,
             "head_pitch_Joint": 0.0,
             "head_yaw_Joint": 0.0,
         },
-        pos=(0.001, 0.001, 0.74),  # z = 0.44 (base clearance) + 0.26 (PrismaticJoint)
+        pos=(0.001, 0.001, 0.7),  # z = 0.44 (base clearance) + 0.26 (PrismaticJoint)
     ),
     actuators={
         # RM-65 J1-J3: shoulder/elbow — max 180°/s ≈ 3.14 rad/s
@@ -110,13 +110,14 @@ MINDBOT_CFG = ArticulationCfg(
         # IdealPDActuator: explicitly computes F = k_p*(target-pos) + k_d*(target_vel-vel)
         # and sends via set_dof_actuation_forces(), bypassing PhysX drive which
         # doesn't work correctly for prismatic joints with set_dof_position_targets().
-        # NOTE: trunk gravity is disabled in USD, so moderate stiffness is sufficient.
+        # NOTE: trunk gravity disabled by startup event. High stiffness to resist
-        "trunk": ImplicitActuatorCfg(
+        # arm reaction forces (~150N) — deflection < 2mm at 100k stiffness.
+        "trunk": IdealPDActuatorCfg(
             joint_names_expr=["PrismaticJoint"],
-            effort_limit=100.0,
+            effort_limit=2000.0,
-            velocity_limit=0.2,
+            velocity_limit=0.5,
             stiffness=100000.0,
-            damping=10000.0,
+            damping=5000.0,
         ),
         "wheels": ImplicitActuatorCfg(
             # joint_names_expr=[".*_revolute_Joint"],