v 1.2
This commit is contained in:
cheng
@@ -21,10 +21,24 @@ class SideTriggerGripController:
|
||||
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()
|
||||
@@ -122,19 +136,92 @@ class SideTriggerGripController:
|
||||
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
|
||||
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
|
||||
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
|
||||
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
|
||||
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:
|
||||
print(f"state={state} action=waiting-for-sensor-samples")
|
||||
action = manual_action or "waiting-for-sensor-samples"
|
||||
print(f"state={state} action={action}")
|
||||
self._emit_status(
|
||||
{
|
||||
"timestamp": time.time(),
|
||||
"state": state,
|
||||
"action": "waiting-for-sensor-samples",
|
||||
"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,
|
||||
@@ -147,9 +234,11 @@ class SideTriggerGripController:
|
||||
grip_contact = self._grip_contact(feedback)
|
||||
normal_change = 0.0
|
||||
shear_change = 0.0
|
||||
action = "wait"
|
||||
action = manual_action or "wait"
|
||||
|
||||
if state == "open_wait":
|
||||
if manual_action is not None:
|
||||
pass
|
||||
elif state == "open_wait":
|
||||
trigger_edge = trigger and not last_trigger
|
||||
if not trigger:
|
||||
if trigger_clear_since is None:
|
||||
@@ -387,6 +476,12 @@ class SideTriggerGripController:
|
||||
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,
|
||||
@@ -577,6 +672,10 @@ class SideTriggerGripController:
|
||||
"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,
|
||||
@@ -586,6 +685,7 @@ class SideTriggerGripController:
|
||||
"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,
|
||||
|
||||
Reference in New Issue
Block a user