cheng
601 lines
24 KiB
Python
601 lines
24 KiB
Python
import time
|
|
import threading
|
|
|
|
from gripper_control.filters import clamp
|
|
|
|
|
|
class SideTriggerGripController:
|
|
def __init__(
|
|
self,
|
|
config,
|
|
reader,
|
|
gripper,
|
|
feedback_processor,
|
|
status_callback=None,
|
|
):
|
|
self.config = config
|
|
self.reader = reader
|
|
self.gripper = gripper
|
|
self.feedback_processor = feedback_processor
|
|
self.normal_unit = feedback_processor.normal_converter.unit
|
|
self.shear_unit = feedback_processor.shear_converter.unit
|
|
self.status_callback = status_callback
|
|
self._stop_event = threading.Event()
|
|
|
|
def stop(self):
|
|
self._stop_event.set()
|
|
|
|
def run(self):
|
|
self._stop_event.clear()
|
|
self.reader.start()
|
|
|
|
try:
|
|
self.gripper.connect()
|
|
open_force = int(clamp(
|
|
self.config.open_force,
|
|
self.config.force_min,
|
|
self.config.force_max,
|
|
))
|
|
print("demo02 start: open gripper and wait for side touch")
|
|
self.gripper.open(open_force, "startup-open")
|
|
|
|
self._print_thresholds()
|
|
print(
|
|
f"Calibrating baseline for {self.config.baseline_seconds:.2f}s. "
|
|
"Keep sensors unloaded."
|
|
)
|
|
baseline = self.feedback_processor.calibrate_baseline(self.reader)
|
|
print(
|
|
f"baseline normal: L={baseline.left_normal:.4f}{self.normal_unit} "
|
|
f"R={baseline.right_normal:.4f}{self.normal_unit}; "
|
|
f"shear: L={baseline.left_shear:.4f}{self.shear_unit} "
|
|
f"R={baseline.right_shear:.4f}{self.shear_unit}"
|
|
)
|
|
|
|
self._run_event_monitor()
|
|
|
|
except KeyboardInterrupt:
|
|
print("\nStopping demo02.")
|
|
finally:
|
|
self.gripper.final_open_if_needed()
|
|
self.reader.stop()
|
|
|
|
def _print_thresholds(self):
|
|
print(
|
|
"Trigger thresholds: "
|
|
f"normal={self.config.trigger_normal_force:.3f}{self.normal_unit}, "
|
|
f"shear={self.config.trigger_shear_force:.3f}{self.shear_unit}"
|
|
)
|
|
print(
|
|
"Grip/release thresholds: "
|
|
f"grip_normal={self.config.grip_normal_force:.3f}{self.normal_unit}, "
|
|
f"requires_both={int(self.config.grip_requires_both)}, "
|
|
f"release_normal_change={self.config.release_normal_change:.3f}{self.normal_unit}, "
|
|
f"release_shear_change={self.config.release_shear_change:.3f}{self.shear_unit}"
|
|
)
|
|
print(
|
|
"Adaptive grip: "
|
|
f"enabled={int(self.config.adaptive_grip_enabled)}, "
|
|
f"shear_start={self.config.adaptive_shear_start_force:.3f}{self.shear_unit}, "
|
|
f"shear_full={self.config.adaptive_shear_full_force:.3f}{self.shear_unit}, "
|
|
f"force={self.config.adaptive_force_min}-{self.config.adaptive_force_max}, "
|
|
f"step={self.config.adaptive_force_step}, "
|
|
f"interval={self.config.adaptive_force_interval:.2f}s, "
|
|
f"release_arm_delay={self.config.release_arm_delay_seconds:.2f}s"
|
|
)
|
|
print(
|
|
"Release open: "
|
|
f"speed={self.config.release_open_speed}, "
|
|
f"accel={self.config.release_open_accel}, "
|
|
f"decel={self.config.release_open_decel}"
|
|
)
|
|
print(
|
|
"Recover: "
|
|
f"force_normal={self.config.recover_normal_force:.3f}{self.normal_unit}, "
|
|
f"force_shear={self.config.recover_shear_force:.3f}{self.shear_unit}, "
|
|
f"open_pos={self.config.open_pos}, "
|
|
f"pos_enabled={int(self.config.open_position_recover_enabled)}, "
|
|
f"pos_tol={self.config.open_position_tolerance}"
|
|
)
|
|
|
|
def _run_event_monitor(self):
|
|
config = self.config
|
|
interval = 1.0 / config.control_hz if config.control_hz > 0 else 0.05
|
|
|
|
state = "open_wait"
|
|
state_start = time.perf_counter()
|
|
last_force_ramp_time = state_start
|
|
last_open_position_check_time = 0.0
|
|
last_open_position = None
|
|
last_open_position_ready = False
|
|
recover_quiet_since = None
|
|
recover_position_since = None
|
|
trigger_clear_since = None
|
|
trigger_armed = False
|
|
last_trigger = False
|
|
force_pct = int(clamp(config.open_force, config.force_min, config.force_max))
|
|
adaptive_target_force = int(clamp(config.hold_force, config.force_min, config.force_max))
|
|
release_armed = False
|
|
hold_pos = None
|
|
reference = None
|
|
|
|
while not self._stop_event.is_set():
|
|
tick_start = time.perf_counter()
|
|
now = time.perf_counter()
|
|
|
|
feedback = self.feedback_processor.read(self.reader, timeout=0.3)
|
|
if feedback is None:
|
|
print(f"state={state} action=waiting-for-sensor-samples")
|
|
self._emit_status(
|
|
{
|
|
"timestamp": time.time(),
|
|
"state": state,
|
|
"action": "waiting-for-sensor-samples",
|
|
"force_pct": force_pct,
|
|
"adaptive_target_force": adaptive_target_force,
|
|
"adaptive_grip_enabled": config.adaptive_grip_enabled,
|
|
"release_armed": release_armed,
|
|
"speed_pct": config.speed,
|
|
"normal_unit": self.normal_unit,
|
|
"shear_unit": self.shear_unit,
|
|
}
|
|
)
|
|
time.sleep(0.05)
|
|
continue
|
|
|
|
trigger = self._side_triggered(feedback)
|
|
grip_contact = self._grip_contact(feedback)
|
|
normal_change = 0.0
|
|
shear_change = 0.0
|
|
action = "wait"
|
|
|
|
if state == "open_wait":
|
|
trigger_edge = trigger and not last_trigger
|
|
if not trigger:
|
|
if trigger_clear_since is None:
|
|
trigger_clear_since = now
|
|
action = "open-wait-trigger-clearing"
|
|
elif now - trigger_clear_since >= config.trigger_clear_stable_seconds:
|
|
trigger_armed = True
|
|
action = "open-wait-armed"
|
|
else:
|
|
action = "open-wait-trigger-clearing"
|
|
elif not trigger_armed:
|
|
trigger_clear_since = None
|
|
action = "open-wait-clear-trigger"
|
|
elif not trigger_edge:
|
|
action = "open-wait-trigger-held"
|
|
else:
|
|
force_pct = int(clamp(
|
|
config.close_force,
|
|
config.force_min,
|
|
config.force_max,
|
|
))
|
|
self.gripper.close(force_pct, "side-trigger-close")
|
|
state = "closing"
|
|
state_start = now
|
|
last_force_ramp_time = now
|
|
last_open_position_check_time = 0.0
|
|
last_open_position = None
|
|
last_open_position_ready = False
|
|
recover_quiet_since = None
|
|
recover_position_since = None
|
|
trigger_clear_since = None
|
|
trigger_armed = False
|
|
adaptive_target_force = self._adaptive_force_target(feedback)
|
|
release_armed = False
|
|
hold_pos = None
|
|
reference = None
|
|
action = "side-trigger-close"
|
|
|
|
elif state == "closing":
|
|
if grip_contact:
|
|
force_pct = int(clamp(
|
|
config.grip_start_force,
|
|
config.force_min,
|
|
config.force_max,
|
|
))
|
|
self.gripper.set_force(force_pct)
|
|
hold_pos = self.gripper.hold_current_position(force_pct)
|
|
state = "gripping"
|
|
state_start = now
|
|
last_force_ramp_time = now
|
|
adaptive_target_force = self._adaptive_force_target(feedback)
|
|
release_armed = False
|
|
reference = None
|
|
action = "object-detected-low-force"
|
|
elif now - state_start >= config.close_timeout_seconds:
|
|
force_pct = int(clamp(
|
|
config.open_force,
|
|
config.force_min,
|
|
config.force_max,
|
|
))
|
|
self.gripper.open(force_pct, "close-timeout-open")
|
|
self.feedback_processor.reset_filters()
|
|
state = "open_recover"
|
|
state_start = now
|
|
last_open_position_check_time = 0.0
|
|
last_open_position = None
|
|
last_open_position_ready = False
|
|
recover_quiet_since = None
|
|
recover_position_since = None
|
|
release_armed = False
|
|
hold_pos = None
|
|
reference = None
|
|
action = "close-timeout-open"
|
|
else:
|
|
action = "closing-to-grip"
|
|
|
|
elif state == "gripping":
|
|
if (
|
|
now - last_force_ramp_time >= config.force_ramp_interval
|
|
and force_pct < config.hold_force
|
|
):
|
|
force_pct = int(clamp(
|
|
force_pct + config.force_ramp_step,
|
|
config.force_min,
|
|
config.hold_force,
|
|
))
|
|
self.gripper.set_force(force_pct)
|
|
last_force_ramp_time = now
|
|
action = "grip-ramp-up"
|
|
elif (
|
|
force_pct >= config.hold_force
|
|
and now - state_start >= config.grip_settle_seconds
|
|
):
|
|
reference = self._make_reference(feedback)
|
|
state = "hold_check"
|
|
state_start = now
|
|
adaptive_target_force = self._adaptive_force_target(feedback)
|
|
release_armed = False
|
|
action = "hold-reference-armed"
|
|
else:
|
|
action = "grip-hold"
|
|
|
|
elif state == "hold_check":
|
|
adaptive_target_force = self._adaptive_force_target(feedback)
|
|
should_adapt_force = (
|
|
config.adaptive_grip_enabled
|
|
and not release_armed
|
|
and force_pct < adaptive_target_force
|
|
)
|
|
|
|
if (
|
|
should_adapt_force
|
|
and now - last_force_ramp_time >= config.adaptive_force_interval
|
|
):
|
|
force_pct = int(clamp(
|
|
force_pct + config.adaptive_force_step,
|
|
config.force_min,
|
|
adaptive_target_force,
|
|
))
|
|
self.gripper.set_force(force_pct)
|
|
last_force_ramp_time = now
|
|
state_start = now
|
|
reference = self._make_reference(feedback)
|
|
release_armed = False
|
|
action = "adaptive-grip-ramp"
|
|
elif should_adapt_force:
|
|
release_armed = False
|
|
action = "adaptive-grip-wait"
|
|
elif (
|
|
not release_armed
|
|
and now - max(state_start, last_force_ramp_time)
|
|
< config.release_arm_delay_seconds
|
|
):
|
|
action = "release-arm-delay"
|
|
elif not release_armed:
|
|
reference = self._make_reference(feedback)
|
|
release_armed = True
|
|
action = "release-reference-armed"
|
|
else:
|
|
normal_change, shear_change = self._release_changes(feedback, reference)
|
|
should_release = (
|
|
normal_change >= config.release_normal_change
|
|
or shear_change >= config.release_shear_change
|
|
)
|
|
if should_release:
|
|
force_pct = int(clamp(
|
|
config.open_force,
|
|
config.force_min,
|
|
config.force_max,
|
|
))
|
|
self.gripper.open(
|
|
force_pct,
|
|
"force-change-release-open",
|
|
speed_pct=config.release_open_speed,
|
|
accel=config.release_open_accel,
|
|
decel=config.release_open_decel,
|
|
)
|
|
self.feedback_processor.reset_filters()
|
|
state = "open_recover"
|
|
state_start = now
|
|
last_open_position_check_time = 0.0
|
|
last_open_position = None
|
|
last_open_position_ready = False
|
|
recover_quiet_since = None
|
|
recover_position_since = None
|
|
release_armed = False
|
|
hold_pos = None
|
|
reference = None
|
|
action = "force-change-release-open"
|
|
else:
|
|
action = "hold-check"
|
|
|
|
elif state == "open_recover":
|
|
current_pos = last_open_position
|
|
position_ready = last_open_position_ready
|
|
if (
|
|
config.open_position_recover_enabled
|
|
and now - last_open_position_check_time
|
|
>= config.open_position_check_interval
|
|
):
|
|
last_open_position_check_time = now
|
|
current_pos = self.gripper.read_position()
|
|
position_ready = self._open_position_ready(current_pos)
|
|
last_open_position = current_pos
|
|
last_open_position_ready = position_ready
|
|
|
|
if position_ready:
|
|
if recover_position_since is None:
|
|
recover_position_since = now
|
|
action = f"open-recover-position pos={current_pos}"
|
|
elif (
|
|
now - recover_position_since
|
|
>= config.open_position_stable_seconds
|
|
and now - state_start >= config.rearm_seconds
|
|
):
|
|
state = "open_wait"
|
|
state_start = now
|
|
recover_quiet_since = None
|
|
recover_position_since = None
|
|
trigger_clear_since = None
|
|
trigger_armed = True
|
|
last_trigger = trigger
|
|
release_armed = False
|
|
hold_pos = None
|
|
reference = None
|
|
action = f"open-ready-position pos={current_pos}"
|
|
else:
|
|
action = f"open-recover-position-stabilizing pos={current_pos}"
|
|
elif self._recover_quiet(feedback):
|
|
recover_position_since = None
|
|
if recover_quiet_since is None:
|
|
recover_quiet_since = now
|
|
action = "open-recover-quiet"
|
|
elif (
|
|
now - recover_quiet_since >= config.recover_stable_seconds
|
|
and now - state_start >= config.rearm_seconds
|
|
):
|
|
state = "open_wait"
|
|
state_start = now
|
|
recover_quiet_since = None
|
|
recover_position_since = None
|
|
trigger_clear_since = None
|
|
trigger_armed = True
|
|
last_trigger = trigger
|
|
release_armed = False
|
|
hold_pos = None
|
|
reference = None
|
|
action = "open-ready"
|
|
else:
|
|
action = "open-recover-stabilizing"
|
|
else:
|
|
recover_quiet_since = None
|
|
recover_position_since = None
|
|
action = "open-recover-wait-force-clear"
|
|
|
|
self._log_tick(
|
|
feedback=feedback,
|
|
state=state,
|
|
trigger=trigger,
|
|
grip_contact=grip_contact,
|
|
normal_change=normal_change,
|
|
shear_change=shear_change,
|
|
hold_pos=hold_pos,
|
|
force_pct=force_pct,
|
|
adaptive_target_force=adaptive_target_force,
|
|
release_armed=release_armed,
|
|
action=action,
|
|
trigger_armed=trigger_armed,
|
|
)
|
|
self._emit_status(
|
|
self._make_status(
|
|
feedback=feedback,
|
|
state=state,
|
|
trigger=trigger,
|
|
grip_contact=grip_contact,
|
|
normal_change=normal_change,
|
|
shear_change=shear_change,
|
|
hold_pos=hold_pos,
|
|
force_pct=force_pct,
|
|
adaptive_target_force=adaptive_target_force,
|
|
release_armed=release_armed,
|
|
action=action,
|
|
trigger_armed=trigger_armed,
|
|
)
|
|
)
|
|
last_trigger = trigger
|
|
|
|
sleep_time = interval - (time.perf_counter() - tick_start)
|
|
if sleep_time > 0:
|
|
time.sleep(sleep_time)
|
|
|
|
print("demo02 control loop stopped.")
|
|
|
|
def _side_triggered(self, feedback):
|
|
return (
|
|
feedback.max_normal >= self.config.trigger_normal_force
|
|
or feedback.max_shear >= self.config.trigger_shear_force
|
|
)
|
|
|
|
def _grip_contact(self, feedback):
|
|
left_contact = feedback.left_normal >= self.config.grip_normal_force
|
|
right_contact = feedback.right_normal >= self.config.grip_normal_force
|
|
if self.config.grip_requires_both:
|
|
return left_contact and right_contact
|
|
return left_contact or right_contact
|
|
|
|
def _recover_quiet(self, feedback):
|
|
return (
|
|
feedback.max_normal <= self.config.recover_normal_force
|
|
and feedback.max_shear <= self.config.recover_shear_force
|
|
)
|
|
|
|
def _open_position_ready(self, current_pos):
|
|
if current_pos is None:
|
|
return False
|
|
return (
|
|
abs(int(current_pos) - int(self.config.open_pos))
|
|
<= int(self.config.open_position_tolerance)
|
|
)
|
|
|
|
def _adaptive_force_target(self, feedback):
|
|
config = self.config
|
|
if not config.adaptive_grip_enabled:
|
|
return int(clamp(config.hold_force, config.force_min, config.force_max))
|
|
|
|
force_min = int(clamp(
|
|
config.adaptive_force_min,
|
|
config.force_min,
|
|
config.force_max,
|
|
))
|
|
force_max = int(clamp(
|
|
config.adaptive_force_max,
|
|
config.force_min,
|
|
config.force_max,
|
|
))
|
|
if force_max < force_min:
|
|
force_max = force_min
|
|
|
|
shear_start = max(0.0, float(config.adaptive_shear_start_force))
|
|
shear_full = max(shear_start + 1e-6, float(config.adaptive_shear_full_force))
|
|
shear = max(0.0, float(feedback.max_shear))
|
|
if shear <= shear_start:
|
|
return force_min
|
|
if shear >= shear_full:
|
|
return force_max
|
|
|
|
ratio = (shear - shear_start) / (shear_full - shear_start)
|
|
target = round(force_min + ratio * (force_max - force_min))
|
|
return int(clamp(target, force_min, force_max))
|
|
|
|
def _make_reference(self, feedback):
|
|
return {
|
|
"left_normal": feedback.left_normal,
|
|
"right_normal": feedback.right_normal,
|
|
"left_shear": feedback.left_shear,
|
|
"right_shear": feedback.right_shear,
|
|
"max_shear": feedback.max_shear,
|
|
}
|
|
|
|
def _release_changes(self, feedback, reference):
|
|
if reference is None:
|
|
return 0.0, 0.0
|
|
normal_change = max(
|
|
abs(feedback.left_normal - reference["left_normal"]),
|
|
abs(feedback.right_normal - reference["right_normal"]),
|
|
)
|
|
shear_change = abs(feedback.max_shear - reference["max_shear"])
|
|
shear_change = max(
|
|
shear_change,
|
|
abs(feedback.left_shear - reference["left_shear"]),
|
|
abs(feedback.right_shear - reference["right_shear"]),
|
|
)
|
|
return normal_change, shear_change
|
|
|
|
def _log_tick(
|
|
self,
|
|
feedback,
|
|
state,
|
|
trigger,
|
|
grip_contact,
|
|
normal_change,
|
|
shear_change,
|
|
hold_pos,
|
|
force_pct,
|
|
adaptive_target_force,
|
|
release_armed,
|
|
action,
|
|
trigger_armed,
|
|
):
|
|
print(
|
|
f"L={feedback.left_normal:7.3f}{self.normal_unit} "
|
|
f"R={feedback.right_normal:7.3f}{self.normal_unit} "
|
|
f"maxN={feedback.max_normal:7.3f}{self.normal_unit} "
|
|
f"shear={feedback.max_shear:7.3f}{self.shear_unit} "
|
|
f"trigger={int(trigger)} armed={int(trigger_armed)} "
|
|
f"grip_contact={int(grip_contact)} "
|
|
f"dN={normal_change:7.3f}{self.normal_unit} "
|
|
f"dShear={shear_change:7.3f}{self.shear_unit} "
|
|
f"state={state} hold_pos={hold_pos} "
|
|
f"force_pct={force_pct:3d} target={adaptive_target_force:3d} "
|
|
f"release_armed={int(release_armed)} action={action}"
|
|
)
|
|
|
|
def _make_status(
|
|
self,
|
|
feedback,
|
|
state,
|
|
trigger,
|
|
grip_contact,
|
|
normal_change,
|
|
shear_change,
|
|
hold_pos,
|
|
force_pct,
|
|
adaptive_target_force,
|
|
release_armed,
|
|
action,
|
|
trigger_armed,
|
|
):
|
|
speed_pct = self.config.speed
|
|
if action == "force-change-release-open":
|
|
speed_pct = self.config.release_open_speed
|
|
|
|
left_sample = None
|
|
right_sample = None
|
|
try:
|
|
if len(self.reader.latest) >= 2:
|
|
left_sample = self.reader.latest[0]
|
|
right_sample = self.reader.latest[1]
|
|
except Exception:
|
|
pass
|
|
|
|
return {
|
|
"timestamp": time.time(),
|
|
"state": state,
|
|
"action": action,
|
|
"trigger": bool(trigger),
|
|
"trigger_armed": bool(trigger_armed),
|
|
"grip_contact": bool(grip_contact),
|
|
"left_normal": feedback.left_normal,
|
|
"right_normal": feedback.right_normal,
|
|
"min_normal": feedback.min_normal,
|
|
"max_normal": feedback.max_normal,
|
|
"normal_diff": feedback.normal_diff,
|
|
"left_shear": feedback.left_shear,
|
|
"right_shear": feedback.right_shear,
|
|
"max_shear": feedback.max_shear,
|
|
"shear_diff": feedback.shear_diff,
|
|
"normal_change": normal_change,
|
|
"shear_change": shear_change,
|
|
"hold_pos": hold_pos,
|
|
"force_pct": force_pct,
|
|
"adaptive_target_force": adaptive_target_force,
|
|
"adaptive_grip_enabled": self.config.adaptive_grip_enabled,
|
|
"release_armed": bool(release_armed),
|
|
"speed_pct": speed_pct,
|
|
"normal_unit": self.normal_unit,
|
|
"shear_unit": self.shear_unit,
|
|
"left_sample": left_sample,
|
|
"right_sample": right_sample,
|
|
}
|
|
|
|
def _emit_status(self, status):
|
|
if self.status_callback is None:
|
|
return
|
|
try:
|
|
self.status_callback(status)
|
|
except Exception as exc:
|
|
print(f"status callback failed: {exc}")
|