import argparse import copy import importlib.util import json from dataclasses import dataclass, field from pathlib import Path from .calibration import ( DEFAULT_NORMAL_FORCE_CALIBRATION, DEFAULT_SHEAR_FORCE_CALIBRATION, ) from .paths import PACKAGE_DIR DEFAULT_CONTROL_CONFIG = str(PACKAGE_DIR / "config" / "gripper_timed_cycle_control.py") def load_control_config(path): suffix = Path(path).suffix.lower() if suffix == ".py": return load_python_config(path) return load_json_config(path) def load_python_config(path): try: spec = importlib.util.spec_from_file_location("gripper_user_config", path) if spec is None or spec.loader is None: raise RuntimeError("cannot create import spec") module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) except FileNotFoundError: print(f"[Info] control config not found: {path}; using CLI/default values") return {} except Exception as exc: print(f"[Error] failed to read control config {path}: {exc}; using CLI/default values") return {} config = getattr(module, "CONFIG", None) if config is None: config = { key.lower(): value for key, value in vars(module).items() if key.isupper() } if not isinstance(config, dict): print(f"[Error] Python config {path} must define CONFIG as a dict; using CLI/default values") return {} return config def load_json_config(path): try: with open(path, "r", encoding="utf-8") as f: return json.load(f) except FileNotFoundError: print(f"[Info] control config not found: {path}; using CLI/default values") return {} except Exception as exc: print(f"[Error] failed to read control config {path}: {exc}; using CLI/default values") return {} def _config_default(config, key, default): return config.get(key, config.get(key.replace("_", "-"), default)) @dataclass class ControlConfig: control_config: str = DEFAULT_CONTROL_CONFIG video1: str = "0" video2: str = "1" rotation1: str = "./config/rotation_config_0.json" rotation2: str = "./config/rotation_config_1.json" sensor_config1: str = "./config/ddjx01.json" sensor_config2: str = "./config/ddjx01.json" sensor_fps: float = 30.0 motion_threshold: float = 0.0 sensor_cpu: bool = False port: str = "COM3" slave_id: int = 1 baudrate: int = 115200 serial_timeout: float = 0.2 open_pos: int = 0 close_pos: int = 9000 open_seconds: float = 3.0 close_seconds: float = 5.0 cycles: int = 0 open_at_end: bool = False speed: int = 20 accel: int = 60 decel: int = 60 initial_force: int = 10 grip_start_force: int = 10 force_min: int = 10 force_max: int = 60 control_hz: float = 10.0 both_contact_force: float = 0.05 object_force: float = 0.08 object_requires_both_contact: bool = True empty_release_seconds: float = 1.0 close_command_interval: float = 0.5 hold_fallback_pos: int | None = None force_ramp_step: int = 1 force_ramp_interval: float = 0.3 shear_hold_force: float = 0.05 shear_stable_delta: float = 0.02 shear_stable_seconds: float = 0.5 human_hold_seconds: float = 1.0 release_shear_change: float = 0.5 emergency_touch_force: float = 2.5 filter_alpha: float = 0.25 shear_filter_alpha: float = 0.35 baseline_seconds: float = 1.0 normal_force_calibration: dict = field( default_factory=lambda: copy.deepcopy(DEFAULT_NORMAL_FORCE_CALIBRATION) ) shear_force_calibration: dict = field( default_factory=lambda: copy.deepcopy(DEFAULT_SHEAR_FORCE_CALIBRATION) ) trigger_force_update: bool = False enable_human_release: bool = True enable_emergency_open: bool = False dry_run: bool = False @classmethod def from_args(cls, argv=None): bootstrap = argparse.ArgumentParser(add_help=False) bootstrap.add_argument("--control-config", default=DEFAULT_CONTROL_CONFIG) bootstrap_args, _ = bootstrap.parse_known_args(argv) config_data = load_control_config(bootstrap_args.control_config) parser = argparse.ArgumentParser() add = parser.add_argument get = lambda key, default: _config_default(config_data, key, default) add("--control-config", type=str, default=bootstrap_args.control_config) add("--video1", "-v1", type=str, default=get("video1", cls.video1)) add("--video2", "-v2", type=str, default=get("video2", cls.video2)) add("--rotation1", "-r1", type=str, default=get("rotation1", cls.rotation1)) add("--rotation2", "-r2", type=str, default=get("rotation2", cls.rotation2)) add("--sensor-config1", type=str, default=get("sensor_config1", cls.sensor_config1)) add("--sensor-config2", type=str, default=get("sensor_config2", cls.sensor_config2)) add("--sensor-fps", type=float, default=get("sensor_fps", cls.sensor_fps)) add("--motion-threshold", type=float, default=get("motion_threshold", cls.motion_threshold)) add("--sensor-cpu", action="store_true", default=get("sensor_cpu", cls.sensor_cpu)) add("--port", type=str, default=get("port", cls.port)) add("--slave-id", type=int, default=get("slave_id", cls.slave_id)) add("--baudrate", type=int, default=get("baudrate", cls.baudrate)) add("--serial-timeout", type=float, default=get("serial_timeout", cls.serial_timeout)) add("--open-pos", type=int, default=get("open_pos", cls.open_pos)) add("--close-pos", type=int, default=get("close_pos", cls.close_pos)) add("--open-seconds", type=float, default=get("open_seconds", cls.open_seconds)) add("--close-seconds", type=float, default=get("close_seconds", cls.close_seconds)) add("--cycles", type=int, default=get("cycles", cls.cycles)) add("--open-at-end", action="store_true", default=get("open_at_end", cls.open_at_end)) add("--speed", type=int, default=get("speed", cls.speed)) add("--accel", type=int, default=get("accel", cls.accel)) add("--decel", type=int, default=get("decel", cls.decel)) add("--initial-force", type=int, default=get("initial_force", cls.initial_force)) add("--grip-start-force", type=int, default=get("grip_start_force", cls.grip_start_force)) add("--force-min", type=int, default=get("force_min", cls.force_min)) add("--force-max", type=int, default=get("force_max", cls.force_max)) add("--control-hz", type=float, default=get("control_hz", cls.control_hz)) add("--both-contact-force", type=float, default=get("both_contact_force", cls.both_contact_force)) add("--object-force", "--clamp-force", dest="object_force", type=float, default=get("object_force", cls.object_force)) add("--object-requires-both-contact", dest="object_requires_both_contact", action="store_true", default=get("object_requires_both_contact", cls.object_requires_both_contact)) add("--allow-single-side-object", dest="object_requires_both_contact", action="store_false") add("--empty-release-seconds", type=float, default=get("empty_release_seconds", cls.empty_release_seconds)) add("--close-command-interval", type=float, default=get("close_command_interval", cls.close_command_interval)) add("--hold-fallback-pos", type=int, default=get("hold_fallback_pos", cls.hold_fallback_pos)) add("--force-ramp-step", type=int, default=get("force_ramp_step", cls.force_ramp_step)) add("--force-ramp-interval", type=float, default=get("force_ramp_interval", cls.force_ramp_interval)) add("--shear-hold-force", type=float, default=get("shear_hold_force", cls.shear_hold_force)) add("--shear-stable-delta", type=float, default=get("shear_stable_delta", cls.shear_stable_delta)) add("--shear-stable-seconds", type=float, default=get("shear_stable_seconds", cls.shear_stable_seconds)) add("--human-hold-seconds", type=float, default=get("human_hold_seconds", cls.human_hold_seconds)) add("--release-shear-change", type=float, default=get("release_shear_change", cls.release_shear_change)) add("--emergency-touch-force", type=float, default=get("emergency_touch_force", cls.emergency_touch_force)) add("--filter-alpha", type=float, default=get("filter_alpha", cls.filter_alpha)) add("--shear-filter-alpha", type=float, default=get("shear_filter_alpha", cls.shear_filter_alpha)) add("--baseline-seconds", type=float, default=get("baseline_seconds", cls.baseline_seconds)) add("--target-touch-force", type=float, default=1500.0, help=argparse.SUPPRESS) add("--deadband", type=float, default=80.0, help=argparse.SUPPRESS) add("--step-up", type=int, default=1, help=argparse.SUPPRESS) add("--step-down", type=int, default=2, help=argparse.SUPPRESS) add("--shear-spike-threshold", type=float, default=0.05, help=argparse.SUPPRESS) add("--trigger-force-update", action="store_true", default=get("trigger_force_update", cls.trigger_force_update)) add("--enable-human-release", dest="enable_human_release", action="store_true", default=get("enable_human_release", cls.enable_human_release)) add("--disable-human-release", dest="enable_human_release", action="store_false") add("--enable-emergency-open", action="store_true", default=get("enable_emergency_open", cls.enable_emergency_open)) add("--dry-run", action="store_true", default=get("dry_run", cls.dry_run)) args = parser.parse_args(argv) values = vars(args) for legacy_key in ("target_touch_force", "deadband", "step_up", "step_down", "shear_spike_threshold"): values.pop(legacy_key, None) values["normal_force_calibration"] = copy.deepcopy( config_data.get("normal_force_calibration", DEFAULT_NORMAL_FORCE_CALIBRATION) ) values["shear_force_calibration"] = copy.deepcopy( config_data.get("shear_force_calibration", DEFAULT_SHEAR_FORCE_CALIBRATION) ) return cls(**values)