cheng
203 lines
11 KiB
Python
203 lines
11 KiB
Python
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
|
|
open_wait_stable_samples: int = 12
|
|
open_wait_stable_range_n: float = 0.03
|
|
open_wait_stable_trend_n: float = 0.015
|
|
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("--open-wait-stable-samples", type=int, default=get("open_wait_stable_samples", cls.open_wait_stable_samples))
|
|
add("--open-wait-stable-range-n", type=float, default=get("open_wait_stable_range_n", cls.open_wait_stable_range_n))
|
|
add("--open-wait-stable-trend-n", type=float, default=get("open_wait_stable_trend_n", cls.open_wait_stable_trend_n))
|
|
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)
|