init
This commit is contained in:
cheng
@@ -0,0 +1,771 @@
|
||||
import time
|
||||
import threading
|
||||
|
||||
from .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()
|
||||
self._manual_lock = threading.Lock()
|
||||
self._manual_command = None
|
||||
|
||||
def stop(self):
|
||||
self._stop_event.set()
|
||||
|
||||
def request_manual_command(self, command):
|
||||
if command not in {"open", "close", "hold", "release"}:
|
||||
raise ValueError(f"unsupported manual command: {command}")
|
||||
with self._manual_lock:
|
||||
self._manual_command = command
|
||||
|
||||
def _pop_manual_command(self):
|
||||
with self._manual_lock:
|
||||
command = self._manual_command
|
||||
self._manual_command = None
|
||||
return command
|
||||
|
||||
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
|
||||
open_wait_samples = []
|
||||
open_wait_stable_values = {}
|
||||
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
|
||||
|
||||
def reset_open_wait_stability():
|
||||
nonlocal open_wait_samples, open_wait_stable_values
|
||||
open_wait_samples = []
|
||||
open_wait_stable_values = {}
|
||||
|
||||
while not self._stop_event.is_set():
|
||||
tick_start = time.perf_counter()
|
||||
now = time.perf_counter()
|
||||
manual_action = None
|
||||
|
||||
manual_command = self._pop_manual_command()
|
||||
if manual_command == "open":
|
||||
force_pct = int(clamp(
|
||||
config.open_force,
|
||||
config.force_min,
|
||||
config.force_max,
|
||||
))
|
||||
self.gripper.open(force_pct, "manual-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
|
||||
trigger_clear_since = None
|
||||
trigger_armed = False
|
||||
reset_open_wait_stability()
|
||||
release_armed = False
|
||||
hold_pos = None
|
||||
reference = None
|
||||
manual_action = "manual-open"
|
||||
elif manual_command == "close":
|
||||
force_pct = int(clamp(
|
||||
config.close_force,
|
||||
config.force_min,
|
||||
config.force_max,
|
||||
))
|
||||
self.gripper.close(force_pct, "manual-close")
|
||||
state = "manual_close"
|
||||
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
|
||||
reset_open_wait_stability()
|
||||
release_armed = False
|
||||
hold_pos = None
|
||||
reference = None
|
||||
manual_action = "manual-close"
|
||||
elif manual_command == "hold":
|
||||
force_pct = int(clamp(
|
||||
config.hold_force,
|
||||
config.force_min,
|
||||
config.force_max,
|
||||
))
|
||||
self.gripper.set_force(force_pct)
|
||||
hold_pos = self.gripper.hold_current_position(force_pct)
|
||||
state = "manual_hold"
|
||||
state_start = now
|
||||
last_force_ramp_time = now
|
||||
trigger_clear_since = None
|
||||
trigger_armed = False
|
||||
reset_open_wait_stability()
|
||||
release_armed = False
|
||||
reference = None
|
||||
manual_action = "manual-hold"
|
||||
elif manual_command == "release":
|
||||
if state == "manual_hold":
|
||||
state = "open_wait"
|
||||
state_start = now
|
||||
trigger_clear_since = None
|
||||
trigger_armed = False
|
||||
reset_open_wait_stability()
|
||||
release_armed = False
|
||||
hold_pos = None
|
||||
reference = None
|
||||
manual_action = "manual-release"
|
||||
|
||||
feedback = self.feedback_processor.read(self.reader, timeout=0.3)
|
||||
if feedback is None:
|
||||
action = manual_action or "waiting-for-sensor-samples"
|
||||
print(f"state={state} action={action}")
|
||||
self._emit_status(
|
||||
{
|
||||
"timestamp": time.time(),
|
||||
"state": state,
|
||||
"action": action,
|
||||
"force_pct": force_pct,
|
||||
"adaptive_target_force": adaptive_target_force,
|
||||
"adaptive_grip_enabled": config.adaptive_grip_enabled,
|
||||
"release_armed": release_armed,
|
||||
"manual_hold_active": state == "manual_hold",
|
||||
"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 = manual_action or "wait"
|
||||
|
||||
if manual_action is not None:
|
||||
pass
|
||||
elif state == "open_wait":
|
||||
open_wait_sample = self._open_wait_sample(feedback)
|
||||
self._append_open_wait_sample(open_wait_samples, open_wait_sample)
|
||||
stable_values = self._open_wait_stable_values(open_wait_samples)
|
||||
trigger = self._open_wait_delta_triggered(
|
||||
open_wait_sample,
|
||||
open_wait_stable_values,
|
||||
)
|
||||
if not trigger:
|
||||
open_wait_stable_values.update(stable_values)
|
||||
|
||||
trigger_armed = bool(open_wait_stable_values)
|
||||
if not trigger_armed:
|
||||
action = "open-wait-stabilizing"
|
||||
elif not trigger:
|
||||
action = "open-wait-armed"
|
||||
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
|
||||
reset_open_wait_stability()
|
||||
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
|
||||
reset_open_wait_stability()
|
||||
release_armed = False
|
||||
hold_pos = None
|
||||
reference = None
|
||||
action = "close-timeout-open"
|
||||
else:
|
||||
if (
|
||||
config.closing_command_interval > 0
|
||||
and now - last_force_ramp_time >= config.closing_command_interval
|
||||
):
|
||||
self.gripper.close(force_pct, "closing-continue")
|
||||
last_force_ramp_time = now
|
||||
action = "closing-continue"
|
||||
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
|
||||
reset_open_wait_stability()
|
||||
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
|
||||
>= max(
|
||||
config.open_position_stable_seconds,
|
||||
config.rearm_seconds,
|
||||
)
|
||||
):
|
||||
state = "open_wait"
|
||||
state_start = now
|
||||
recover_quiet_since = None
|
||||
recover_position_since = None
|
||||
trigger_clear_since = None
|
||||
trigger_armed = True
|
||||
reset_open_wait_stability()
|
||||
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
|
||||
>= max(config.recover_stable_seconds, config.rearm_seconds)
|
||||
):
|
||||
state = "open_wait"
|
||||
state_start = now
|
||||
recover_quiet_since = None
|
||||
recover_position_since = None
|
||||
trigger_clear_since = None
|
||||
trigger_armed = True
|
||||
reset_open_wait_stability()
|
||||
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"
|
||||
|
||||
elif state == "manual_hold":
|
||||
action = "manual-hold"
|
||||
|
||||
elif state == "manual_close":
|
||||
action = "manual-close"
|
||||
|
||||
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 _open_wait_sample(self, feedback):
|
||||
return {
|
||||
"left_normal": feedback.left_normal,
|
||||
"right_normal": feedback.right_normal,
|
||||
"left_shear": feedback.left_shear,
|
||||
"right_shear": feedback.right_shear,
|
||||
}
|
||||
|
||||
def _append_open_wait_sample(self, samples, sample):
|
||||
samples.append(sample)
|
||||
stable_samples = max(2, int(self.config.open_wait_stable_samples))
|
||||
if len(samples) > stable_samples:
|
||||
del samples[0]
|
||||
|
||||
def _open_wait_stable_values(self, samples):
|
||||
stable_samples = max(2, int(self.config.open_wait_stable_samples))
|
||||
if len(samples) < stable_samples:
|
||||
return {}
|
||||
|
||||
stable = {}
|
||||
first = samples[0]
|
||||
last = samples[-1]
|
||||
for key in first:
|
||||
values = [sample[key] for sample in samples]
|
||||
value_range = max(values) - min(values)
|
||||
trend = abs(last[key] - first[key])
|
||||
if (
|
||||
value_range <= self.config.open_wait_stable_range_n
|
||||
and trend <= self.config.open_wait_stable_trend_n
|
||||
):
|
||||
stable[key] = sum(values) / len(values)
|
||||
return stable
|
||||
|
||||
def _open_wait_delta_triggered(self, sample, stable_values):
|
||||
thresholds = {
|
||||
"left_normal": self.config.trigger_normal_force,
|
||||
"right_normal": self.config.trigger_normal_force,
|
||||
"left_shear": self.config.trigger_shear_force,
|
||||
"right_shear": self.config.trigger_shear_force,
|
||||
}
|
||||
for key, stable_value in stable_values.items():
|
||||
if sample[key] - stable_value >= thresholds[key]:
|
||||
return True
|
||||
return False
|
||||
|
||||
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,
|
||||
"left_shear_x": feedback.left_shear_x,
|
||||
"left_shear_y": feedback.left_shear_y,
|
||||
"right_shear_x": feedback.right_shear_x,
|
||||
"right_shear_y": feedback.right_shear_y,
|
||||
"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),
|
||||
"manual_hold_active": state == "manual_hold",
|
||||
"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}")
|
||||
Reference in New Issue
Block a user