""" Closed-loop gripper force control from two tactile sensors. Run from repository root: python examples/gripper_force_control.py --port COM3 -v1 0 -v2 1 The controller reads left/right tactile normal force and updates the gripper target force percentage. It uses the larger side as the safety signal because the current gripper SDK exposes one shared force command. """ import argparse import os import sys import time from pathlib import Path import multiprocessing as mp from two_sensor_force_reader import TwoSensorForceReader REPO_ROOT = Path(__file__).resolve().parents[1] SDK_DIR = REPO_ROOT / "sdk" if str(SDK_DIR) not in sys.path: sys.path.insert(0, str(SDK_DIR)) class ExponentialFilter: def __init__(self, alpha, initial=0.0): self.alpha = max(0.0, min(1.0, float(alpha))) self.value = float(initial) self.initialized = False def update(self, sample): sample = float(sample) if not self.initialized: self.value = sample self.initialized = True else: self.value = self.alpha * sample + (1.0 - self.alpha) * self.value return self.value def clamp(value, min_value, max_value): return max(min_value, min(max_value, value)) def step_toward_open(current_pos, open_pos, delta): if current_pos is None: return open_pos if current_pos > open_pos: return max(open_pos, current_pos - delta) if current_pos < open_pos: return min(open_pos, current_pos + delta) return open_pos def calibrate_baseline(reader, seconds): if seconds <= 0: return 0.0, 0.0 left_values = [] right_values = [] deadline = time.perf_counter() + seconds while time.perf_counter() < deadline: left, right = reader.get_forces(timeout=0.2) if left is not None: left_values.append(left) if right is not None: right_values.append(right) time.sleep(0.01) left_baseline = sum(left_values) / len(left_values) if left_values else 0.0 right_baseline = sum(right_values) / len(right_values) if right_values else 0.0 return left_baseline, right_baseline def compute_next_force_pct( current_pct, control_force, target_force, deadband, force_min, force_max, step_up, step_down, ): low = target_force - deadband high = target_force + deadband if control_force > high: return clamp(current_pct - step_down, force_min, force_max), "down" if control_force < low: return clamp(current_pct + step_up, force_min, force_max), "up" return current_pct, "hold" def load_gripper(args): from changingtek_p_rtu_Servo import MotorController return MotorController( args.port, args.slave_id, baudrate=args.baudrate, timeout=args.serial_timeout, ) def parse_args(): parser = argparse.ArgumentParser() parser.add_argument("--video1", "-v1", type=str, default="0", help="left sensor camera index or video path") parser.add_argument("--video2", "-v2", type=str, default="1", help="right sensor camera index or video path") parser.add_argument("--rotation1", "-r1", type=str, default="./config/rotation_config_0.json", help="left sensor orientation config") parser.add_argument("--rotation2", "-r2", type=str, default="./config/rotation_config_1.json", help="right sensor orientation config") parser.add_argument("--sensor-config1", type=str, default="./config/ddjx01.json", help="left tactile sensor config") parser.add_argument("--sensor-config2", type=str, default="./config/ddjx01.json", help="right tactile sensor config") parser.add_argument("--sensor-fps", type=float, default=30.0, help="target processing FPS per tactile sensor") parser.add_argument("--motion-threshold", type=float, default=0.0, help="motion threshold used by tactile deformation") parser.add_argument("--sensor-cpu", action="store_true", help="disable CUDA for tactile workers") parser.add_argument("--port", type=str, default="COM5", help="gripper serial port") parser.add_argument("--slave-id", type=int, default=1, help="Modbus slave id") parser.add_argument("--baudrate", type=int, default=115200, help="serial baudrate") parser.add_argument("--serial-timeout", type=float, default=0.2, help="serial timeout in seconds") parser.add_argument("--open-pos", type=int, default=0, help="open gripper position") parser.add_argument("--close-pos", type=int, default=9000, help="close gripper position") parser.add_argument("--speed", type=int, default=5, help="gripper speed percent") parser.add_argument("--accel", type=int, default=60, help="gripper acceleration") parser.add_argument("--decel", type=int, default=60, help="gripper deceleration") parser.add_argument("--initial-force", type=int, default=10, help="initial gripper force percent") parser.add_argument("--force-min", type=int, default=5, help="minimum gripper force percent") parser.add_argument("--force-max", type=int, default=35, help="maximum gripper force percent") parser.add_argument("--step-up", type=int, default=1, help="force percent increase per control tick") parser.add_argument("--step-down", type=int, default=2, help="force percent decrease per control tick") parser.add_argument("--control-hz", type=float, default=10.0, help="gripper force update frequency") parser.add_argument("--target-touch-force", type=float, default=800.0, help="target tactile normal force value") parser.add_argument("--deadband", type=float, default=80.0, help="do not adjust inside target +/- deadband") parser.add_argument("--emergency-touch-force", type=float, default=1600.0, help="open slightly if either side exceeds this value") parser.add_argument("--emergency-open-delta", type=int, default=500, help="position step toward open on emergency") parser.add_argument("--imbalance-threshold", type=float, default=0.0, help="optional left/right force difference warning threshold; 0 disables") parser.add_argument("--filter-alpha", type=float, default=0.25, help="force low-pass filter alpha") parser.add_argument("--baseline-seconds", type=float, default=1.0, help="seconds to sample no-contact tactile baseline") parser.add_argument("--no-initial-move", action="store_true", help="do not send initial close command") parser.add_argument("--trigger-force-update", action="store_true", help="call trigger_motion after set_target_force") parser.add_argument("--dry-run", action="store_true", help="print commands without opening the serial port") return parser.parse_args() def main(): args = parse_args() reader = TwoSensorForceReader( video1=args.video1, video2=args.video2, config1=args.sensor_config1, config2=args.sensor_config2, rotation1=args.rotation1, rotation2=args.rotation2, target_fps=args.sensor_fps, cuda=not args.sensor_cpu, motion_threshold=args.motion_threshold, ) gripper = None force_pct = int(clamp(args.initial_force, args.force_min, args.force_max)) left_filter = ExponentialFilter(args.filter_alpha) right_filter = ExponentialFilter(args.filter_alpha) last_written_force = None print("Starting tactile sensors...") reader.start() try: print(f"Calibrating tactile baseline for {args.baseline_seconds:.2f}s. Keep sensors unloaded.") left_base, right_base = calibrate_baseline(reader, args.baseline_seconds) print(f"baseline: left={left_base:.4f}, right={right_base:.4f}") if not args.dry_run: gripper = load_gripper(args) if not args.no_initial_move: if args.dry_run: print( "[dry-run] temp_move " f"position={args.close_pos}, speed={args.speed}, force={force_pct}, " f"accel={args.accel}, decel={args.decel}" ) else: gripper.temp_move( position_mm=args.close_pos, speed_pct=args.speed, force_pct=force_pct, accel=args.accel, decel=args.decel, trigger=True, ) last_written_force = force_pct interval = 1.0 / args.control_hz if args.control_hz > 0 else 0.1 print("Closed-loop control started. Press Ctrl+C to stop.") while True: tick_start = time.perf_counter() left_raw, right_raw = reader.get_forces(timeout=1.0) if left_raw is None or right_raw is None: print(f"waiting for forces: left={left_raw}, right={right_raw}") time.sleep(0.05) continue left_force = max(0.0, float(left_raw) - left_base) right_force = max(0.0, float(right_raw) - right_base) left_filtered = left_filter.update(left_force) right_filtered = right_filter.update(right_force) control_force = max(left_filtered, right_filtered) avg_force = 0.5 * (left_filtered + right_filtered) imbalance = left_filtered - right_filtered if control_force >= args.emergency_touch_force: force_pct = args.force_min action = "emergency" if args.dry_run: print(f"[dry-run] set_target_force {force_pct}") print(f"[dry-run] open toward {args.open_pos} by {args.emergency_open_delta}") else: gripper.set_target_force(force_pct) try: current_pos = gripper.read_real_position() except Exception: current_pos = None open_pos = step_toward_open( current_pos, args.open_pos, args.emergency_open_delta, ) gripper.temp_move( position_mm=int(open_pos), speed_pct=args.speed, force_pct=force_pct, accel=args.accel, decel=args.decel, trigger=True, ) last_written_force = force_pct else: force_pct, action = compute_next_force_pct( force_pct, control_force, args.target_touch_force, args.deadband, args.force_min, args.force_max, args.step_up, args.step_down, ) if force_pct != last_written_force: if args.dry_run: print(f"[dry-run] set_target_force {force_pct}") else: gripper.set_target_force(int(force_pct)) if args.trigger_force_update: gripper.trigger_motion() last_written_force = force_pct imbalance_msg = "" if args.imbalance_threshold > 0 and abs(imbalance) >= args.imbalance_threshold: imbalance_msg = " imbalance" print( f"L={left_filtered:8.2f} R={right_filtered:8.2f} " f"avg={avg_force:8.2f} max={control_force:8.2f} " f"diff={imbalance:8.2f} force_pct={force_pct:3d} " f"action={action}{imbalance_msg}" ) elapsed = time.perf_counter() - tick_start sleep_time = interval - elapsed if sleep_time > 0: time.sleep(sleep_time) except KeyboardInterrupt: print("\nStopping closed-loop control.") finally: reader.stop() if __name__ == "__main__": mp.freeze_support() main()