From 95be03914cf8f264d2f4d411f8b61069d58c67b2 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Wed, 17 Jun 2026 00:51:12 +0900 Subject: [PATCH 1/3] =?UTF-8?q?=ED=95=B8=EB=93=9C=EC=98=A4=EB=B2=84=20?= =?UTF-8?q?=EB=A7=88=EB=AC=B4=EB=A6=AC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../azas_task_manager/auto_cup_flow_router.py | 25 +- tools/run/auto_handover_on_palm.py | 26 +- tools/run/handover_cup_to_palm.py | 259 ++++++++++++++++-- tools/run/robot_pipeline_control_server.py | 15 +- 4 files changed, 275 insertions(+), 50 deletions(-) diff --git a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py index a30ace7..9841865 100644 --- a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py +++ b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py @@ -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 " diff --git a/tools/run/auto_handover_on_palm.py b/tools/run/auto_handover_on_palm.py index 071c810..d0b91a0 100755 --- a/tools/run/auto_handover_on_palm.py +++ b/tools/run/auto_handover_on_palm.py @@ -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, @@ -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") @@ -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}", diff --git a/tools/run/handover_cup_to_palm.py b/tools/run/handover_cup_to_palm.py index ac945ff..70f811b 100755 --- a/tools/run/handover_cup_to_palm.py +++ b/tools/run/handover_cup_to_palm.py @@ -54,6 +54,17 @@ def prefixed_service(prefix: str, suffix: str) -> str: return f"/{clean}/{suffix}" if clean else f"/{suffix}" +def clamp(value: float, lower: float, upper: float) -> float: + return min(max(value, lower), upper) + + +def parse_ikin_sol_spaces(value: str) -> list[int]: + values = [int(part.strip()) for part in str(value).split(",") if part.strip()] + if not values: + raise ValueError("--ikin-sol-spaces did not contain any solution spaces") + return values + + def resolve_service_prefix(node, srv_type, requested_prefix: str, wait_sec: float, *, allow_fallback: bool) -> str: requested = requested_prefix.strip("/") if requested or not allow_fallback: @@ -72,7 +83,7 @@ class HandoverPerception: def __init__(self, args: argparse.Namespace) -> None: import rclpy import tf2_ros - from dsr_msgs2.srv import GetCurrentPosx, GetToolForce + from dsr_msgs2.srv import GetCurrentPosx, GetToolForce, Ikin from geometry_msgs.msg import PointStamped from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy @@ -97,6 +108,7 @@ def __init__(self, args: argparse.Namespace) -> None: self.get_tool_force = self.node.create_client( GetToolForce, prefixed_service(prefix, "aux_control/get_tool_force") ) + self.ikin = self.node.create_client(Ikin, prefixed_service(prefix, "motion/ikin")) self.hand_points: list[tuple[float, list[float]]] = [] hand_qos = QoSProfile( history=HistoryPolicy.KEEP_LAST, @@ -161,6 +173,60 @@ def averaged_tool_force_n(self, *, samples: int, interval_sec: float) -> list[fl time.sleep(max(interval_sec, 0.0)) return [value / count for value in total] + def ikin_pose_ok( + self, + xyz_m: list[float], + rpy_deg: list[float], + *, + args: argparse.Namespace, + label: str, + ) -> tuple[bool, str]: + from dsr_msgs2.srv import Ikin + + timeout_sec = max(float(args.ikin_timeout_sec), 0.1) + if not self.ikin.wait_for_service(timeout_sec=timeout_sec): + return False, f"{label}: Ikin service unavailable" + try: + sol_spaces = parse_ikin_sol_spaces(args.ikin_sol_spaces) + except ValueError as exc: + return False, str(exc) + + pos_mm_deg = [ + xyz_m[0] * 1000.0, + xyz_m[1] * 1000.0, + xyz_m[2] * 1000.0, + rpy_deg[0], + rpy_deg[1], + rpy_deg[2], + ] + last_failure = "" + for sol_space in sol_spaces: + req = Ikin.Request() + req.pos = pos_mm_deg + req.sol_space = int(sol_space) + req.ref = 0 # DR_BASE + future = self.ikin.call_async(req) + self.rclpy.spin_until_future_complete(self.node, future, timeout_sec=timeout_sec) + if not future.done(): + last_failure = f"sol_space={sol_space} timeout after {timeout_sec:.1f}s" + continue + if future.exception() is not None: + last_failure = f"sol_space={sol_space} exception: {future.exception()}" + continue + response = future.result() + if response is None or not bool(response.success): + last_failure = f"sol_space={sol_space} success=false" + continue + joints = [float(value) for value in list(response.conv_posj)] + if len(joints) >= 5 and not float(args.j5_min_deg) <= joints[4] <= float(args.j5_max_deg): + last_failure = ( + f"sol_space={sol_space} joint_5={joints[4]:.1f} outside " + f"[{float(args.j5_min_deg):.1f}, {float(args.j5_max_deg):.1f}]" + ) + continue + return True, f"sol_space={sol_space}" + return False, last_failure or "no IK solution" + def base_to_camera(self) -> np.ndarray: import rclpy.time @@ -390,6 +456,112 @@ def require_typed_approval(phrase: str, *, prompt: str, preapproved: str = "") - raise RuntimeError(f"operator approval mismatch; expected {phrase}") +def bound_handover_xy(x: float, y: float, args: argparse.Namespace) -> list[float]: + x = clamp(x, args.handover_target_x_min_m, args.handover_target_x_max_m) + y = clamp(y, args.handover_target_y_min_m, args.handover_target_y_max_m) + radius = math.hypot(x, y) + min_radius = max(float(args.handover_target_xy_radius_min_m), 0.0) + max_radius = max(float(args.handover_target_xy_radius_max_m), min_radius + 1e-6) + if radius > max_radius: + scale = max_radius / radius + x *= scale + y *= scale + elif 1e-6 < radius < min_radius: + scale = min_radius / radius + x *= scale + y *= scale + return [ + clamp(x, args.handover_target_x_min_m, args.handover_target_x_max_m), + clamp(y, args.handover_target_y_min_m, args.handover_target_y_max_m), + ] + + +def handover_descent_z_limits(palm_z: float, args: argparse.Namespace) -> tuple[float, float]: + start_z = palm_z + max(float(args.force_search_start_above_palm_m), 0.0) + if args.release_on_contact: + floor_z = max(float(args.z_min), palm_z - max(float(args.force_search_below_palm_m), 0.0)) + else: + floor_z = palm_z + float(args.release_tcp_above_palm_m) + return start_z, floor_z + + +def ik_probe_poses_for_handover_target( + xy: list[float], + palm_z: float, + args: argparse.Namespace, +) -> list[list[float]]: + start_z, floor_z = handover_descent_z_limits(palm_z, args) + z_values = [start_z, floor_z] + if abs(start_z - floor_z) > 0.04: + z_values.insert(1, (start_z + floor_z) * 0.5) + return [[xy[0], xy[1], clamp(z, args.z_min, args.z_max)] for z in z_values] + + +def select_ik_reachable_handover_target( + perception: HandoverPerception, + palm: list[float], + current_m: list[float], + rpy_deg: list[float], + args: argparse.Namespace, +) -> list[float]: + nearest_xy = bound_handover_xy(palm[0], palm[1], args) + current_xy = bound_handover_xy(current_m[0], current_m[1], args) + max_adjust = max(float(args.max_handover_target_adjust_m), 0.0) + last_failure = "" + seen: set[tuple[float, float]] = set() + + # Start at the closest bounded point to the detected palm. If that is still + # outside the robot's IK envelope, pull the target toward the current robot + # side-grip pose in small increments and use the first fully IK-valid point. + for blend in (0.0, 0.10, 0.20, 0.35, 0.50, 0.65, 0.80, 1.0): + xy = [ + nearest_xy[0] + (current_xy[0] - nearest_xy[0]) * blend, + nearest_xy[1] + (current_xy[1] - nearest_xy[1]) * blend, + ] + xy = bound_handover_xy(xy[0], xy[1], args) + key = (round(xy[0], 4), round(xy[1], 4)) + if key in seen: + continue + seen.add(key) + adjust_m = math.dist([palm[0], palm[1]], xy) + if max_adjust > 0.0 and adjust_m > max_adjust: + last_failure = ( + f"candidate x={xy[0]:.3f} y={xy[1]:.3f} is " + f"{adjust_m * 1000.0:.0f}mm from detected palm, above " + f"max_handover_target_adjust={max_adjust * 1000.0:.0f}mm" + ) + continue + + probe_poses = ik_probe_poses_for_handover_target(xy, palm[2], args) + failures = [] + for index, pose in enumerate(probe_poses, start=1): + ok, detail = perception.ikin_pose_ok(pose, rpy_deg, args=args, label=f"handover_target_probe_{index}") + if not ok: + failures.append(f"z={pose[2]:.3f}: {detail}") + if not failures: + if adjust_m > 0.001: + print( + "[Azas] handover target adjusted toward IK-valid workspace: " + f"palm_xy=[{palm[0]:.3f}, {palm[1]:.3f}] " + f"target_xy=[{xy[0]:.3f}, {xy[1]:.3f}] " + f"offset={adjust_m * 1000.0:.0f}mm" + ) + else: + print("[Azas] detected palm XY is inside the IK-checked handover target envelope") + return [xy[0], xy[1], palm[2]] + last_failure = "; ".join(failures) + print( + "[Azas] IK rejected handover target candidate: " + f"x={xy[0]:.3f} y={xy[1]:.3f} offset={adjust_m * 1000.0:.0f}mm; " + f"{last_failure}" + ) + + raise RuntimeError( + "No IK-valid handover target found near the detected palm. " + f"Last failure: {last_failure}. Move the palm closer to the robot's front-center handover area." + ) + + def parse_args() -> argparse.Namespace: parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) parser.add_argument("--service-prefix", default=os.environ.get("SERVICE_PREFIX", "")) @@ -417,38 +589,40 @@ def parse_args() -> argparse.Namespace: parser.add_argument("--force-search-below-palm-m", type=float, default=0.10, help="with --release-on-contact, search down to this far below the detected palm before aborting") parser.add_argument("--descent-step-m", type=float, default=0.03) + parser.add_argument("--first-descent-step-m", type=float, default=0.08, + help="first Z descent step before force/contact checks; subsequent steps use --descent-step-m") 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-abort-delta-n", type=float, default=2.0, + parser.add_argument("--force-abort-delta-n", type=float, default=0.6, help="abort descent when |tool force| rises this much over the pre-descent baseline") - 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="trigger contact when the monitored 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 handover release") + 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="store_true", help="during staged descent, treat a force rise as palm contact: stop, open RG2, then retreat") parser.add_argument("--require-contact-for-release", action=argparse.BooleanOptionalAction, default=True, help="with --release-on-contact, only open RG2 after contact is 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, preserving the strict default") - 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") @@ -468,6 +642,20 @@ def parse_args() -> argparse.Namespace: parser.add_argument("--z-max", type=float, default=0.75) parser.add_argument("--palm-z-max-m", type=float, default=0.50, help="reject palms higher than this (likely a mis-detection)") + parser.add_argument("--handover-target-x-min-m", type=float, default=0.18, + help="minimum IK-biased TCP target x for handover; detected palm is not rewritten") + parser.add_argument("--handover-target-x-max-m", type=float, default=0.65, + help="maximum IK-biased TCP target x for handover") + parser.add_argument("--handover-target-y-min-m", type=float, default=-0.55, + help="minimum IK-biased TCP target y for handover") + parser.add_argument("--handover-target-y-max-m", type=float, default=0.55, + help="maximum IK-biased TCP target y for handover") + parser.add_argument("--handover-target-xy-radius-min-m", type=float, default=0.25, + help="minimum base XY radius for the IK-biased handover target") + parser.add_argument("--handover-target-xy-radius-max-m", type=float, default=0.62, + help="maximum base XY radius for the IK-biased handover target") + parser.add_argument("--max-handover-target-adjust-m", type=float, default=0.20, + help="fail closed if the IK-valid handover target would be farther from the detected palm") parser.add_argument("--move-timeout-sec", type=float, default=60.0) parser.add_argument("--verify-timeout-sec", type=float, default=90.0) parser.add_argument("--target-tolerance-mm", type=float, default=25.0) @@ -527,25 +715,45 @@ def main() -> int: and args.z_min <= palm[2] <= min(args.z_max, args.palm_z_max_m)): print(f"[BLOCKED] palm outside handover workspace bounds; refusing: palm={palm}") return 1 + preserved_rpy = current[3:6] + handover_target = select_ik_reachable_handover_target( + perception, + palm, + current_m, + preserved_rpy, + args, + ) lift = [current_m[0], current_m[1], max(current_m[2], args.transit_z_m)] - contact_start_z = palm[2] + max(args.force_search_start_above_palm_m, 0.0) - descent_start_z = contact_start_z if args.release_on_contact else palm[2] + args.above_palm_m + contact_start_z = handover_target[2] + max(args.force_search_start_above_palm_m, 0.0) + descent_start_z = ( + contact_start_z + if args.release_on_contact + else handover_target[2] + args.above_palm_m + ) if args.diagonal_approach: approach_z = max(args.z_min, min(args.z_max, descent_start_z)) approach_label = "APPROACH" else: approach_z = max(args.transit_z_m, descent_start_z) approach_label = "ABOVE_HIGH" - approach = [palm[0], palm[1], approach_z] + approach = [handover_target[0], handover_target[1], approach_z] above_palm = [ - palm[0], - palm[1], + handover_target[0], + handover_target[1], descent_start_z, ] - release = [palm[0], palm[1], palm[2] + args.release_tcp_above_palm_m] - contact_floor_z = max(args.z_min, palm[2] - max(args.force_search_below_palm_m, 0.0)) - retreat = [palm[0], palm[1], palm[2] + args.retreat_lift_m] + release = [ + handover_target[0], + handover_target[1], + handover_target[2] + args.release_tcp_above_palm_m, + ] + contact_floor_z = max(args.z_min, handover_target[2] - max(args.force_search_below_palm_m, 0.0)) + retreat = [ + handover_target[0], + handover_target[1], + handover_target[2] + args.retreat_lift_m, + ] plan_items = [("LIFT", lift), (approach_label, approach), ("ABOVE_PALM", above_palm)] if not args.release_on_contact: plan_items.append(("RELEASE", release)) @@ -563,7 +771,8 @@ def main() -> int: "[PLAN] descent ABOVE_PALM -> " f"{'CONTACT_SEARCH_FLOOR' if args.release_on_contact else 'RELEASE'} in " f"{math.ceil((above_palm[2] - (contact_floor_z if args.release_on_contact else release[2])) / max(args.descent_step_m, 0.005))} steps of " - f"{args.descent_step_m * 1000.0:.0f}mm with force abort delta {args.force_abort_delta_n:.1f}N" + f"{args.first_descent_step_m * 1000.0:.0f}mm first / " + f"{args.descent_step_m * 1000.0:.0f}mm subsequent with force abort delta {args.force_abort_delta_n:.1f}N" ) if args.max_descent_steps > 0: no_contact_action = "retreat with cup" if args.require_contact_for_release else "open at final descent pose" @@ -585,7 +794,6 @@ def main() -> int: ), preapproved=args.approve_motion, ) - preserved_rpy = current[3:6] run_movel(args, lift, label="LIFT to transit height (Z-only)", velocity=args.transit_velocity, acceleration=args.transit_acceleration, rpy_deg=preserved_rpy) @@ -644,8 +852,9 @@ def main() -> int: print(f"[Azas] max descent steps reached ({args.max_descent_steps}) without confirmed contact") break descent_step_index += 1 - z = max(z - max(args.descent_step_m, 0.005), descent_floor_z) - run_movel(args, [palm[0], palm[1], z], label=f"descent step to z={z:.3f}m", + descent_step_m = args.first_descent_step_m if descent_step_index == 1 else args.descent_step_m + z = max(z - max(descent_step_m, 0.005), descent_floor_z) + run_movel(args, [handover_target[0], handover_target[1], z], label=f"descent step to z={z:.3f}m", velocity=args.descent_velocity, acceleration=args.descent_acceleration, rpy_deg=preserved_rpy) if not args.skip_force_monitor: diff --git a/tools/run/robot_pipeline_control_server.py b/tools/run/robot_pipeline_control_server.py index ba98d2b..1a963ac 100755 --- a/tools/run/robot_pipeline_control_server.py +++ b/tools/run/robot_pipeline_control_server.py @@ -3837,7 +3837,20 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe f"--release-tcp-above-palm-m {shlex.quote(release_height_m)} " "--transit-velocity 10.0 --transit-acceleration 14.0 " "--descent-velocity 4.0 --descent-acceleration 6.0 " - "--force-abort-delta-n 10.0 " + "--first-descent-step-m 0.080 " + "--release-on-contact " + "--require-contact-for-release " + "--contact-axis all " + "--contact-z-direction any " + "--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 " + "--force-read-settle-sec 0.05 " + "--contact-confirm-samples 2 " + "--contact-confirm-min-hits 1 " + "--contact-confirm-interval-sec 0.05 " "--execute --confirm ENABLE_HUMAN_PALM_HANDOVER " "--approve-motion ENABLE_HUMAN_PALM_HANDOVER_MOTION " "--approve-release RELEASE_CUP_NOW" From ad45d3ff150ea0663fb4c9cea05356646898e2c1 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Wed, 17 Jun 2026 11:28:03 +0900 Subject: [PATCH 2/3] Update route stability parameters to enhance cup detection reliability --- .../launch/auto_cup_flow_router.launch.py | 2 +- .../azas_cup_uprighting/_base_node.py | 47 ++++++++++++-- .../azas_cup_uprighting/_config.py | 9 +++ .../yolo_cup_uprighting_node.py | 61 +++++++++++++------ .../azas_task_manager/auto_cup_flow_router.py | 2 +- tools/run/run_stt_order_then_router.sh | 2 +- tools/run/run_voice_auto_cup_flow.sh | 2 +- 7 files changed, 96 insertions(+), 29 deletions(-) diff --git a/src/azas_bringup/launch/auto_cup_flow_router.launch.py b/src/azas_bringup/launch/auto_cup_flow_router.launch.py index 9023ea1..d21f4b9 100644 --- a/src/azas_bringup/launch/auto_cup_flow_router.launch.py +++ b/src/azas_bringup/launch/auto_cup_flow_router.launch.py @@ -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=""), diff --git a/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py b/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py index 4e93d86..cf2bfef 100644 --- a/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py +++ b/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py @@ -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") @@ -85,8 +86,10 @@ 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 # ── Hand-Eye ── self.gripper2cam, calib_file = perc.load_hand_eye() @@ -288,6 +291,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 + 재검출 # ════════════════════════════════════════════ @@ -375,6 +387,14 @@ 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 @@ -382,6 +402,7 @@ def _work(): success = bool(self.detect_and_pick(frame)) finally: self._frozen_frame = None + self._frozen_detections = 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() @@ -477,18 +498,34 @@ def run(self): 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 diff --git a/src/azas_cup_uprighting/azas_cup_uprighting/_config.py b/src/azas_cup_uprighting/azas_cup_uprighting/_config.py index e18b4a0..6352689 100644 --- a/src/azas_cup_uprighting/azas_cup_uprighting/_config.py +++ b/src/azas_cup_uprighting/azas_cup_uprighting/_config.py @@ -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) diff --git a/src/azas_cup_uprighting/azas_cup_uprighting/yolo_cup_uprighting_node.py b/src/azas_cup_uprighting/azas_cup_uprighting/yolo_cup_uprighting_node.py index 4379a59..a2253b6 100644 --- a/src/azas_cup_uprighting/azas_cup_uprighting/yolo_cup_uprighting_node.py +++ b/src/azas_cup_uprighting/azas_cup_uprighting/yolo_cup_uprighting_node.py @@ -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) @@ -142,7 +150,13 @@ 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) @@ -150,21 +164,28 @@ def detect_and_pick(self, frame: np.ndarray): 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: @@ -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: diff --git a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py index 9841865..27855b8 100644 --- a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py +++ b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py @@ -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") diff --git a/tools/run/run_stt_order_then_router.sh b/tools/run/run_stt_order_then_router.sh index f316cc0..256dc7f 100755 --- a/tools/run/run_stt_order_then_router.sh +++ b/tools/run/run_stt_order_then_router.sh @@ -54,7 +54,7 @@ run_one_order() { classifier_arch:=resnet18 \ route_hold_sec:=2.0 \ route_stable_required_samples:=5 \ - route_stable_min_sec:=0.8 + route_stable_min_sec:=2.0 } if [[ "${LOOP}" == "true" ]]; then diff --git a/tools/run/run_voice_auto_cup_flow.sh b/tools/run/run_voice_auto_cup_flow.sh index 61783f4..7b8d887 100755 --- a/tools/run/run_voice_auto_cup_flow.sh +++ b/tools/run/run_voice_auto_cup_flow.sh @@ -58,7 +58,7 @@ ros2 launch azas_bringup auto_cup_flow_router.launch.py \ classifier_arch:=resnet18 \ route_hold_sec:=2.0 \ route_stable_required_samples:=5 \ - route_stable_min_sec:=0.8 \ + route_stable_min_sec:=3.0 \ recipe_colors:="${RECIPE_COLORS}" \ resume_mode:="${AUTO_FLOW_RESUME_MODE}" \ resume_state_file:="${AUTO_FLOW_RESUME_STATE_FILE}" \ From 9ccabfdf1baf95c9193995ad479a6a3c8de6dc45 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Wed, 17 Jun 2026 16:39:30 +0900 Subject: [PATCH 3/3] Add shutdown handling after pick completion in BaseMoveItPickNode --- .../azas_cup_uprighting/_base_node.py | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py b/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py index cf2bfef..8df8bf6 100644 --- a/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py +++ b/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py @@ -90,6 +90,7 @@ def __init__(self): 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() @@ -401,11 +402,15 @@ def _work(): try: success = bool(self.detect_and_pick(frame)) finally: - self._frozen_frame = None - self._frozen_detections = 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() @@ -492,6 +497,10 @@ 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