From eef7ff838d1ae588224e782bd376bb8afee79248 Mon Sep 17 00:00:00 2001 From: yt lee Date: Wed, 25 Mar 2026 16:32:52 +0800 Subject: [PATCH] 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 --- .../teleoperation/xr_teleop/diagnostics.py | 9 ++++++++- .../xr_teleop/dual_arm_head_agent.py | 10 ++++++++-- .../il/open_drybox/mindrobot_2i_cfg.py | 15 ++++++++------- 3 files changed, 24 insertions(+), 10 deletions(-) diff --git a/scripts/environments/teleoperation/xr_teleop/diagnostics.py b/scripts/environments/teleoperation/xr_teleop/diagnostics.py index b77173b..22b0986 100644 --- a/scripts/environments/teleoperation/xr_teleop/diagnostics.py +++ b/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() diff --git a/scripts/environments/teleoperation/xr_teleop/dual_arm_head_agent.py b/scripts/environments/teleoperation/xr_teleop/dual_arm_head_agent.py index c3559e5..440311a 100644 --- a/scripts/environments/teleoperation/xr_teleop/dual_arm_head_agent.py +++ b/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): diff --git a/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_2i_cfg.py b/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_2i_cfg.py index 7bfd579..671c563 100644 --- a/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_2i_cfg.py +++ b/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. - "trunk": ImplicitActuatorCfg( + # NOTE: trunk gravity disabled by startup event. High stiffness to resist + # arm reaction forces (~150N) — deflection < 2mm at 100k stiffness. + "trunk": IdealPDActuatorCfg( joint_names_expr=["PrismaticJoint"], - effort_limit=100.0, - velocity_limit=0.2, + effort_limit=2000.0, + velocity_limit=0.5, stiffness=100000.0, - damping=10000.0, + damping=5000.0, ), "wheels": ImplicitActuatorCfg( # joint_names_expr=[".*_revolute_Joint"],