init commit

workflows/simbox/core/controllers/README.md

@@ -0,0 +1,101 @@
+# Arm Controllers
+
+Template-based arm controllers for CuRobo motion planning. All arm controllers inherit from `TemplateController` and customize behavior via overrides.
+
+## Available controllers
+
+| Controller | Robot | Notes |
+|------------|--------|------|
+| `FR3Controller` | Franka FR3 (Panda arm, 7+1 gripper) | Single arm only; larger collision cache (1000). |
+| `FrankaRobotiq85Controller` | Franka + Robotiq 85 (7+2 gripper) | Single arm only; custom gripper action (inverted, clip 0–5). |
+| `Genie1Controller` | Genie1 dual arm (7 DoF per arm) | Left/right via `robot_file`; path-selection weights. |
+| `Lift2Controller` | ARX-Lift2 dual arm (6 DoF arm) | Left/right; custom world (cuboid offset 0.02), grasp axis 0 (x), in-plane rotation index 5. |
+| `SplitAlohaController` | Agilex Split Aloha dual arm (6 DoF per arm) | Left/right; grasp axis 2 (z); optional `joint_ctrl`. |
+
+Register a controller by importing it (see `__init__.py`) so it is added to `CONTROLLER_DICT`.
+
+---
+
+## Customizing a robot arm controller
+
+Subclass `TemplateController` and implement or override the following.
+
+### 1. Required: `_configure_joint_indices(self, robot_file: str)`
+
+Set joint names and indices for the planner and the simulation articulation.
+
+You must set:
+
+- **`self.raw_js_names`** – Joint names in the **planner / CuRobo** order (arm only, no gripper). Used for `get_ordered_joint_state(raw_js_names)` when building `cmd_plan`.
+- **`self.cmd_js_names`** – Same as `raw_js_names` use the **scene/articulation** names in the robot usd (e.g. `fl_joint1`… or `idx21_arm_l_joint1`…).
+- **`self.arm_indices`** – Indices of arm joints in the **simulation** `dof_names` (e.g. `np.array([0,1,2,3,4,5,6])`).
+- **`self.gripper_indices`** – Indices of gripper joints in the simulation (e.g. `np.array([7])` or `np.array([7,8])`).
+- **`self.reference_prim_path`** – Prim path used for collision reference (e.g. `self.task.robots[self.name].fl_base_path` for left arm).
+- **`self.lr_name`** – `"left"` or `"right"` (for dual-arm).
+- **`self._gripper_state`** – Initial gripper state from robot (e.g. `1.0 if self.robot.left_gripper_state == 1.0 else -1.0`). By convention, `1.0` means **open**, `-1.0` means **closed**.
+- **`self._gripper_joint_position`** – Initial gripper joint position(s), shape matching `gripper_indices` (e.g. `np.array([1.0])` or `np.array([5.0, 5.0])`).
+
+For dual-arm, branch on `"left"` / `"right"` in `robot_file` and set the above per arm. For single-arm, only implement the arm you support and we set it as left arm.
+
+### 2. Required: `_get_default_ignore_substring(self) -> List[str]`
+
+Return the default list of name substrings for collision filtering (e.g. `["material", "Plane", "conveyor", "scene", "table"]`). The controller name is appended automatically. Override to add or remove terms (e.g. `"fluid"` for some setups).
+
+### 3. Required: `get_gripper_action(self)`
+
+Map the logical gripper state to gripper joint targets.
+
+- **Input**: uses `self._gripper_state` (1.0 = open, -1.0 = closed) and `self._gripper_joint_position` as the magnitude / joint-space template.
+- **Default mapping**: for simple parallel grippers, a good starting point is:
+
+  ```python
+  def get_gripper_action(self):
+      return np.clip(self._gripper_state * self._gripper_joint_position, 0.0, 0.04)
+  ```
+
+- **Robot-specific variants**: some robots change the range or sign (e.g. Robotiq85 uses two joints, inverted sign, and clips to `[0, 5]`). Adjust the formula and clip range, but keep the convention that `self._gripper_state` is `1.0` for **open** and `-1.0` for **closed**.
+
+### 4. Optional overrides
+
+Override only when the default template behavior is wrong for your robot.
+
+- **`_load_world(self, use_default: bool = True)`**
+  Default uses `WorldConfig()` when `use_default=True`, and when `False` uses a table with cuboid z offset `10.5`. Override if your table height or world is different (e.g. Genie1 uses `5.02`, Lift2 uses `0.02`).
+
+- **`_get_motion_gen_collision_cache(self)`**
+  Default: `{"obb": 700, "mesh": 700}`. Override to change cache size (e.g. FR3 uses `1000`).
+
+- **`_get_grasp_approach_linear_axis(self) -> int`**
+  Default: `2` (z-axis). Override if your grasp approach constraint uses another axis (e.g. Lift2 uses `0` for x).
+
+- **`_get_sort_path_weights(self) -> Optional[List[float]]`**
+  Default: `None` (equal weights). Override to pass per-joint weights for batch path selection (e.g. Genie1 uses `[1,1,1,1,3,3,1]` for 7 joints).
+
+- **`get_gripper_action(self)`**
+  Default: `np.clip(self._gripper_state * self._gripper_joint_position, 0.0, 0.04)`. Override if your gripper mapping or range differs (e.g. Robotiq85: inverted sign and clip to `[0, 5]`).
+
+### 5. Registration
+
+- Use the `@register_controller` decorator on your class.
+- Import the new controller in `__init__.py` so it is registered in `CONTROLLER_DICT`.
+
+### 6. Robot config (YAML)
+
+Your robot must have a CuRobo config YAML (passed as `robot_file`) with at least:
+
+- `robot_cfg` with `kinematics` (e.g. `urdf_path`, `base_link`, `ee_link`).
+
+Template uses this for `_load_robot`, `_load_kin_model`, and `_init_motion_gen`; no code change needed if the YAML is correct.
+
+---
+
+## Summary checklist for a new arm
+
+1. Add a new file, e.g. `myrobot_controller.py`.
+2. Subclass `TemplateController` and apply `@register_controller`.
+3. Implement **`_configure_joint_indices(robot_file)`** (joint names and indices for planner and sim).
+4. Implement **`_get_default_ignore_substring()`** (collision ignore list).
+5. Implement **`get_gripper_action(self)`** to map `self._gripper_state` (1.0 = open, -1.0 = closed) and `self._gripper_joint_position` to gripper joint targets (clip to a sensible range for your hardware).
+6. Override **`_load_world`** only if table/world differs from default.
+7. Override **`_get_motion_gen_collision_cache`** / **`_get_grasp_approach_linear_axis`** / **`_get_sort_path_weights`** only if needed.
+8. Import the new controller in `__init__.py`.