import argparse import copy from dataclasses import dataclass, field from gripper_control.calibration import ( DEFAULT_NORMAL_FORCE_CALIBRATION, DEFAULT_SHEAR_FORCE_CALIBRATION, ) from gripper_control.config import load_control_config from gripper_control.paths import REPO_ROOT DEFAULT_CONTROL_CONFIG = str( REPO_ROOT / "gripper_control_02" / "config" / "gripper_demo_02.py" ) def _config_default(config, key, default): return config.get(key, config.get(key.replace("_", "-"), default)) @dataclass class Demo02Config: 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 = "COM5" slave_id: int = 1 baudrate: int = 115200 serial_timeout: float = 0.2 open_pos: int = 0 close_pos: int = 9000 speed: int = 25 accel: int = 80 decel: int = 80 release_open_speed: int = 50 release_open_accel: int = 120 release_open_decel: int = 120 open_force: int = 10 close_force: int = 10 grip_start_force: int = 10 hold_force: int = 15 initial_force: int = 10 force_min: int = 10 force_max: int = 60 control_hz: float = 30.0 baseline_seconds: float = 1.0 filter_alpha: float = 0.55 shear_filter_alpha: float = 0.65 trigger_normal_force: float = 0.04 trigger_shear_force: float = 0.04 trigger_clear_stable_seconds: float = 0.2 grip_normal_force: float = 0.05 grip_requires_both: bool = True close_timeout_seconds: float = 5.0 force_ramp_step: int = 1 force_ramp_interval: float = 0.1 grip_settle_seconds: float = 0.1 adaptive_grip_enabled: bool = True adaptive_shear_start_force: float = 0.10 adaptive_shear_full_force: float = 0.60 adaptive_force_min: int = 15 adaptive_force_max: int = 45 adaptive_force_step: int = 1 adaptive_force_interval: float = 0.15 release_arm_delay_seconds: float = 0.8 release_normal_change: float = 0.03 release_shear_change: float = 0.03 rearm_seconds: float = 0.5 recover_normal_force: float = 0.02 recover_shear_force: float = 0.02 recover_stable_seconds: float = 0.4 open_position_recover_enabled: bool = True open_position_tolerance: int = 100 open_position_stable_seconds: float = 0.1 open_position_check_interval: float = 0.1 hold_fallback_pos: int | None = None trigger_force_update: bool = False open_at_end: bool = True dry_run: bool = False 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) ) @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("--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("--release-open-speed", type=int, default=get("release_open_speed", cls.release_open_speed)) add("--release-open-accel", type=int, default=get("release_open_accel", cls.release_open_accel)) add("--release-open-decel", type=int, default=get("release_open_decel", cls.release_open_decel)) add("--open-force", type=int, default=get("open_force", cls.open_force)) add("--close-force", type=int, default=get("close_force", cls.close_force)) add("--grip-start-force", type=int, default=get("grip_start_force", cls.grip_start_force)) add("--hold-force", type=int, default=get("hold_force", cls.hold_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("--baseline-seconds", type=float, default=get("baseline_seconds", cls.baseline_seconds)) 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("--trigger-normal-force", type=float, default=get("trigger_normal_force", cls.trigger_normal_force)) add("--trigger-shear-force", type=float, default=get("trigger_shear_force", cls.trigger_shear_force)) add("--trigger-clear-stable-seconds", type=float, default=get("trigger_clear_stable_seconds", cls.trigger_clear_stable_seconds)) add("--grip-normal-force", type=float, default=get("grip_normal_force", cls.grip_normal_force)) add("--grip-requires-both", dest="grip_requires_both", action="store_true", default=get("grip_requires_both", cls.grip_requires_both)) add("--allow-single-side-grip", dest="grip_requires_both", action="store_false") add("--close-timeout-seconds", type=float, default=get("close_timeout_seconds", cls.close_timeout_seconds)) 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("--grip-settle-seconds", type=float, default=get("grip_settle_seconds", cls.grip_settle_seconds)) add("--enable-adaptive-grip", dest="adaptive_grip_enabled", action="store_true", default=get("adaptive_grip_enabled", cls.adaptive_grip_enabled)) add("--disable-adaptive-grip", dest="adaptive_grip_enabled", action="store_false") add("--adaptive-shear-start-force", type=float, default=get("adaptive_shear_start_force", cls.adaptive_shear_start_force)) add("--adaptive-shear-full-force", type=float, default=get("adaptive_shear_full_force", cls.adaptive_shear_full_force)) add("--adaptive-force-min", type=int, default=get("adaptive_force_min", cls.adaptive_force_min)) add("--adaptive-force-max", type=int, default=get("adaptive_force_max", cls.adaptive_force_max)) add("--adaptive-force-step", type=int, default=get("adaptive_force_step", cls.adaptive_force_step)) add("--adaptive-force-interval", type=float, default=get("adaptive_force_interval", cls.adaptive_force_interval)) add("--release-arm-delay-seconds", type=float, default=get("release_arm_delay_seconds", cls.release_arm_delay_seconds)) add("--release-normal-change", type=float, default=get("release_normal_change", cls.release_normal_change)) add("--release-shear-change", type=float, default=get("release_shear_change", cls.release_shear_change)) add("--rearm-seconds", type=float, default=get("rearm_seconds", cls.rearm_seconds)) add("--recover-normal-force", type=float, default=get("recover_normal_force", cls.recover_normal_force)) add("--recover-shear-force", type=float, default=get("recover_shear_force", cls.recover_shear_force)) add("--recover-stable-seconds", type=float, default=get("recover_stable_seconds", cls.recover_stable_seconds)) add("--open-position-recover-enabled", action="store_true", default=get("open_position_recover_enabled", cls.open_position_recover_enabled)) add("--disable-open-position-recover", dest="open_position_recover_enabled", action="store_false") add("--open-position-tolerance", type=int, default=get("open_position_tolerance", cls.open_position_tolerance)) add("--open-position-stable-seconds", type=float, default=get("open_position_stable_seconds", cls.open_position_stable_seconds)) add("--open-position-check-interval", type=float, default=get("open_position_check_interval", cls.open_position_check_interval)) add("--hold-fallback-pos", type=int, default=get("hold_fallback_pos", cls.hold_fallback_pos)) add("--trigger-force-update", action="store_true", default=get("trigger_force_update", cls.trigger_force_update)) add("--open-at-end", action="store_true", default=get("open_at_end", cls.open_at_end)) add("--dry-run", action="store_true", default=get("dry_run", cls.dry_run)) args = parser.parse_args(argv) values = vars(args) values["initial_force"] = values["open_force"] 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)