Skip to content

Fix/handover sidegrip mediapipe #61

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
oyeong011 wants to merge 3 commits into
base: main
Choose a base branch
from
Open

Fix/handover sidegrip mediapipe #61

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
95be039
핸드오버 마무리
oyeong011 Jun 16, 2026
ad45d3f
Update route stability parameters to enhance cup detection reliability
oyeong011 Jun 17, 2026
9ccabfd
Add shutdown handling after pick completion in BaseMoveItPickNode
oyeong011 Jun 17, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion src/azas_bringup/launch/auto_cup_flow_router.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@ def generate_launch_description():
DeclareLaunchArgument("route_timeout_sec", default_value="30.0"),
DeclareLaunchArgument("route_hold_sec", default_value="3.5"),
DeclareLaunchArgument("route_stable_required_samples", default_value="5"),
DeclareLaunchArgument("route_stable_min_sec", default_value="0.8"),
DeclareLaunchArgument("route_stable_min_sec", default_value="2.0"),
DeclareLaunchArgument("show_classification_window", default_value="true"),
DeclareLaunchArgument("side_extra_args", default_value=""),
DeclareLaunchArgument("cup_uprighting_extra_args", default_value=""),
Expand Down
64 changes: 55 additions & 9 deletions src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -72,6 +72,7 @@ def __init__(self):

# ── 픽 상태 ──
self.declare_parameter("auto_pick", False)
self.declare_parameter("auto_pick_stable_min_sec", 3.0)
self.declare_parameter("exit_after_pick", False)
self.declare_parameter("skip_initial_home_move", False)
self.declare_parameter("controller_action_name", "/dsr01/dsr_moveit_controller/follow_joint_trajectory")
Expand All @@ -85,8 +86,11 @@ def __init__(self):
self.get_parameter("skip_initial_home_move").value
)
self._last_pick_time = 0.0
self._auto_pick_candidate_since = 0.0
self._detections: list[dict] = []
self._frozen_frame = None
self._frozen_detections: list[dict] | None = None
self._shutdown_after_pick_requested = False

# ── Hand-Eye ──
self.gripper2cam, calib_file = perc.load_hand_eye()
Expand Down Expand Up @@ -288,6 +292,15 @@ def go_home_pose(self) -> bool:
home_state.update()
return self.plan_state(home_state)

def go_robot_home_pose(self) -> bool:
"""카메라 관측 자세가 아닌 로봇 기본 home 자세로 이동."""
if not self._ensure_moveit():
return False
home_state = RobotState(self.robot_model)
home_state.joint_positions = cfg.ROBOT_HOME_JOINTS
home_state.update()
return self.plan_state(home_state)

# ════════════════════════════════════════════
# Approach + 재검출
# ════════════════════════════════════════════
Expand Down Expand Up @@ -375,16 +388,29 @@ def _pick_in_thread(self, frame: np.ndarray):
if self.picking:
return
self._frozen_frame = frame.copy()
self._frozen_detections = [dict(d) for d in self._detections]
direction_snapshots = sum(
1 for det in self._frozen_detections if "cup_grasp_theta_rad" in det
)
self.get_logger().info(
"pick 시작: frozen frame/detection snapshot을 유지하고 완료 전까지 새 카메라 방향 인식을 생략합니다. "
f"direction_snapshots={direction_snapshots}/{len(self._frozen_detections)}"
)

def _work():
success = False
try:
success = bool(self.detect_and_pick(frame))
finally:
self._frozen_frame = None
if success and self._exit_after_pick:
self.get_logger().info("exit_after_pick=true and pick completed; closing cup_uprighting node")
rclpy.shutdown()
if success and self._exit_after_pick:
self._shutdown_after_pick_requested = True
self._auto_mode = False
self._auto_pick_candidate_since = 0.0
self.get_logger().info("exit_after_pick=true and pick completed; closing cup_uprighting node")
rclpy.shutdown()
else:
self._frozen_frame = None
self._frozen_detections = None

threading.Thread(target=_work, daemon=True).start()

Expand Down Expand Up @@ -471,24 +497,44 @@ def run(self):
continue

# ── Live 분기 ──
if self._shutdown_after_pick_requested:
time.sleep(0.01)
continue

if self.color_image is None:
time.sleep(0.01)
continue

frame = self.color_image.copy()
self._detections = self.run_yolo(frame)
target = self._select_target(self._detections)
vis = self._draw_detections(frame)

now = time.time()
if (self._auto_mode
and not self.picking
and self.is_auto_ready()
and (now - self._last_pick_time) >= cfg.AUTO_PICK_INTERVAL):
if self._select_target(self._detections) is not None:
self._last_pick_time = now
self._pick_in_thread(frame)
continue
if target is None:
self._auto_pick_candidate_since = 0.0
else:
if self._auto_pick_candidate_since <= 0.0:
self._auto_pick_candidate_since = now
self.get_logger().info(
"[AUTO] 컵 방향 안정 관측 시작: "
f"{float(self.get_parameter('auto_pick_stable_min_sec').value):.1f}s 후 frozen"
)
stable_elapsed = now - self._auto_pick_candidate_since
stable_min_sec = max(
0.0,
float(self.get_parameter("auto_pick_stable_min_sec").value),
)
if stable_elapsed >= stable_min_sec:
self._last_pick_time = now
self._auto_pick_candidate_since = 0.0
self._pick_in_thread(frame)
continue

vis = self._draw_detections(frame)
cv2.imshow(self.WINDOW_NAME, vis)

key = cv2.waitKey(1) & 0xFF
Expand Down
9 changes: 9 additions & 0 deletions src/azas_cup_uprighting/azas_cup_uprighting/_config.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,15 @@ def load_yaml(file_name):
"joint_6": math.radians(90.0),
}

ROBOT_HOME_JOINTS = {
"joint_1": math.radians(0.0),
"joint_2": math.radians(0.0),
"joint_3": math.radians(90.0),
"joint_4": math.radians(0.0),
"joint_5": math.radians(90.0),
"joint_6": math.radians(90.0),
}


# ── Pick 파라미터 (m) ────────────────────────────────
Z_OFFSET = 0.20 # gripper tip ↔ link_6 (depth 측정 base z + 이 값 = pick_z)
Expand Down
61 changes: 41 additions & 20 deletions src/azas_cup_uprighting/azas_cup_uprighting/yolo_cup_uprighting_node.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -106,13 +106,21 @@ def _draw_detections(self, frame: np.ndarray) -> np.ndarray:
x1, y1, x2, y2 = det["box"]
cx, cy = det["cx"], det["cy"]

# 컵 주축 각도 계산
theta = calculate_cup_orientation(self.depth_image, det["box"], frame)

# 입구 방향 판별
is_top = is_top_pointing_towards_theta(frame, det["box"], theta)

top_theta = theta if is_top else theta + np.pi
if "cup_grasp_theta_rad" in det and "cup_axis_theta_rad" in det:
theta = float(det["cup_axis_theta_rad"])
is_top = bool(det.get("cup_top_aligned_with_axis", True))
top_theta = float(det["cup_grasp_theta_rad"])
else:
# 컵 주축 각도 계산
theta = calculate_cup_orientation(self.depth_image, det["box"], frame)

# 입구 방향 판별
is_top = is_top_pointing_towards_theta(frame, det["box"], theta)

top_theta = theta if is_top else theta + np.pi
det["cup_axis_theta_rad"] = float(theta)
det["cup_top_aligned_with_axis"] = bool(is_top)
det["cup_grasp_theta_rad"] = float(top_theta)

length = max(x2 - x1, y2 - y1) // 2
dx = int(np.cos(theta) * length)
Expand Down Expand Up @@ -142,29 +150,42 @@ def detect_and_pick(self, frame: np.ndarray):
log.warn("이미 시퀀스 실행 중입니다.")
return

detections = self.run_yolo(frame)
frozen_frame = self._frozen_frame if self._frozen_frame is not None else frame
frame_snapshot = frozen_frame.copy()
if self._frozen_detections is not None:
detections = [dict(d) for d in self._frozen_detections]
log.info("[VISION] frozen detection snapshot 사용: YOLO 재실행 생략")
else:
detections = self.run_yolo(frame_snapshot)
self._detections = detections
target = self._select_target(detections)

if target is None:
log.warn("쓰러진 컵을 찾을 수 없습니다.")
return

if "cup_grasp_theta_rad" in target:
cup_theta = float(target["cup_grasp_theta_rad"])
is_top = bool(target.get("cup_top_aligned_with_axis", True))
if is_top:
log.info("[VISION] frozen 파지 방향 snapshot 사용: 정방향, 방향 재계산 생략")
else:
log.info("[VISION] frozen 파지 방향 snapshot 사용: 반대 방향, 180도 뒤집은 값을 유지")
else:
cup_theta = calculate_cup_orientation(self.depth_image, target["box"], frame_snapshot)
is_top = is_top_pointing_towards_theta(frame_snapshot, target["box"], cup_theta)

if not is_top:
log.info("[VISION] 컵이 반대로 누워있습니다. 카메라 상향 유지를 위해 파지 방향을 180도 뒤집습니다.")
cup_theta += np.pi
else:
log.info("[VISION] 컵이 정방향입니다. 기본 파지 방향을 유지합니다.")

base = self.pixel_to_base(target["cx"], target["cy"])
if base is None:
log.error("픽셀 -> 베이스 3D 좌표 변환 실패.")
return
bx, by, bz = base

cup_theta = calculate_cup_orientation(self.depth_image, target["box"], frame)

is_top = is_top_pointing_towards_theta(frame, target["box"], cup_theta)

if not is_top:
log.info("[VISION] 컵이 반대로 누워있습니다. 카메라 상향 유지를 위해 파지 방향을 180도 뒤집습니다.")
cup_theta += np.pi
else:
log.info("[VISION] 컵이 정방향입니다. 기본 파지 방향을 유지합니다.")

self.picking = True
try:
Expand Down Expand Up @@ -231,8 +252,8 @@ def _pick_and_return_home(self, bx, by, bz, cup_theta):
return False
time.sleep(1.0)

log.info("[4] 홈 위치로 복귀 (파지 유지)")
if self.go_home_pose():
log.info("[4] 로봇 홈 위치로 복귀 (카메라 관측 자세 아님, 파지 유지)")
if self.go_robot_home_pose():
log.info("=> 홈 복귀 성공. 전체 구출 시퀀스 완수.")
return True
else:
Expand Down
27 changes: 14 additions & 13 deletions src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -74,7 +74,7 @@ def __init__(self) -> None:
self.declare_parameter("classifier_min_confidence", 0.70)
self.declare_parameter("route_timeout_sec", 30.0)
self.declare_parameter("route_stable_required_samples", 5)
self.declare_parameter("route_stable_min_sec", 0.8)
self.declare_parameter("route_stable_min_sec", 2.0)
self.declare_parameter("route_hold_sec", 3.5)
self.declare_parameter("show_classification_window", True)
self.declare_parameter("window_name", "Azas cup route classifier")
Expand Down Expand Up @@ -916,24 +916,25 @@ def _human_handover_command(self) -> str:
"--hand-sample-spread-max-m 0.05 "
"--skip-hand-recheck "
"--release-on-contact "
"--no-require-contact-for-release "
"--require-contact-for-release "
"--force-search-start-above-palm-m 0.16 "
"--force-search-below-palm-m 0.10 "
"--max-descent-steps 10 "
"--contact-axis z "
"--contact-z-direction positive "
"--contact-axis all "
"--contact-z-direction any "
"--force-baseline-samples 5 "
"--force-baseline-interval-sec 0.05 "
"--force-read-settle-sec 0.08 "
"--force-abort-delta-n 3.5 "
"--force-axis-delta-n 3.5 "
"--contact-step-delta-n 2.5 "
"--require-force-magnitude-delta "
"--force-magnitude-delta-n 2.0 "
"--contact-confirm-samples 3 "
"--contact-confirm-min-hits 3 "
"--contact-confirm-interval-sec 0.08 "
"--force-read-settle-sec 0.05 "
"--force-abort-delta-n 0.6 "
"--force-axis-delta-n 0.5 "
"--contact-step-delta-n 0.3 "
"--no-require-force-magnitude-delta "
"--force-magnitude-delta-n 0.6 "
"--contact-confirm-samples 2 "
"--contact-confirm-min-hits 1 "
"--contact-confirm-interval-sec 0.05 "
"--descent-step-m 0.030 "
"--first-descent-step-m 0.080 "
"--transit-velocity 55 "
"--transit-acceleration 75 "
"--descent-velocity 22 "
Expand Down
26 changes: 14 additions & 12 deletions tools/run/auto_handover_on_palm.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -198,6 +198,7 @@ def main() -> int:
parser.add_argument("--descent-velocity", type=float, default=22.0)
parser.add_argument("--descent-acceleration", type=float, default=32.0)
parser.add_argument("--descent-step-m", type=float, default=0.03)
parser.add_argument("--first-descent-step-m", type=float, default=0.08)
parser.add_argument("--max-descent-steps", type=int, default=0,
help="maximum staged descent steps; 0 means use the Z floor only")
parser.add_argument("--force-search-start-above-palm-m", type=float, default=0.16,
Expand All @@ -214,36 +215,36 @@ def main() -> int:
parser.add_argument("--j5-max-deg", type=float, default=None)
parser.add_argument("--skip-force-monitor", action="store_true",
help="pass through to handover script; staged descent remains but force abort is disabled")
parser.add_argument("--force-abort-delta-n", type=float, default=2.0,
parser.add_argument("--force-abort-delta-n", type=float, default=0.6,
help="force rise over baseline that counts as palm contact during descent")
parser.add_argument("--force-axis-delta-n", type=float, default=1.0,
parser.add_argument("--force-axis-delta-n", type=float, default=0.5,
help="also count contact when any single force axis changes by this much")
parser.add_argument("--contact-axis", choices=("z", "xy", "all"), default="z",
help="force axes used for contact release; z is safest for vertical handover")
parser.add_argument("--contact-z-direction", choices=("positive", "negative", "any"), default="positive",
parser.add_argument("--contact-axis", choices=("z", "xy", "all"), default="all",
help="force axes used for contact release; all is most sensitive for vertical handover")
parser.add_argument("--contact-z-direction", choices=("positive", "negative", "any"), default="any",
help="when --contact-axis z, require this signed Z force delta for contact")
parser.add_argument("--contact-step-delta-n", type=float, default=2.0,
parser.add_argument("--contact-step-delta-n", type=float, default=0.3,
help="contact candidate also requires this force jump from the previous descent step")
parser.add_argument("--require-force-magnitude-delta", action=argparse.BooleanOptionalAction, default=True,
parser.add_argument("--require-force-magnitude-delta", action=argparse.BooleanOptionalAction, default=False,
help="also require total force magnitude to rise before contact release")
parser.add_argument("--force-magnitude-delta-n", type=float, default=1.5,
parser.add_argument("--force-magnitude-delta-n", type=float, default=0.6,
help="minimum total force magnitude rise required with --require-force-magnitude-delta")
parser.add_argument("--force-baseline-samples", type=int, default=5,
help="average this many GetToolForce samples before descent")
parser.add_argument("--force-baseline-interval-sec", type=float, default=0.05,
help="delay between baseline force samples")
parser.add_argument("--force-read-settle-sec", type=float, default=0.15,
parser.add_argument("--force-read-settle-sec", type=float, default=0.05,
help="wait after each descent step before reading force")
parser.add_argument("--release-on-contact", action=argparse.BooleanOptionalAction, default=True,
help="open the gripper at the first force/contact trigger during descent")
parser.add_argument("--require-contact-for-release", action=argparse.BooleanOptionalAction, default=True,
help="with --release-on-contact, retreat with the cup if contact is never detected")
parser.add_argument("--contact-confirm-samples", type=int, default=5,
parser.add_argument("--contact-confirm-samples", type=int, default=2,
help="consecutive above-threshold force samples required before opening RG2")
parser.add_argument("--contact-confirm-min-hits", type=int, default=0,
parser.add_argument("--contact-confirm-min-hits", type=int, default=1,
help="minimum hit samples needed within --contact-confirm-samples; "
"0 means all samples")
parser.add_argument("--contact-confirm-interval-sec", type=float, default=0.12,
parser.add_argument("--contact-confirm-interval-sec", type=float, default=0.05,
help="delay between force confirmation samples")
parser.add_argument("--contact-relief-lift-m", type=float, default=0.0,
help="deprecated/ignored: contact release now opens RG2 at the confirmed contact pose")
Expand Down Expand Up @@ -293,6 +294,7 @@ def main() -> int:
"--descent-velocity", f"{args.descent_velocity:.3f}",
"--descent-acceleration", f"{args.descent_acceleration:.3f}",
"--descent-step-m", f"{args.descent_step_m:.3f}",
"--first-descent-step-m", f"{args.first_descent_step_m:.3f}",
"--max-descent-steps", str(args.max_descent_steps),
"--force-search-start-above-palm-m", f"{args.force_search_start_above_palm_m:.3f}",
"--force-search-below-palm-m", f"{args.force_search_below_palm_m:.3f}",
Expand Down
Loading
Loading