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