diff --git a/COMMANDS.md b/COMMANDS.md index cc56223..7963f53 100644 --- a/COMMANDS.md +++ b/COMMANDS.md @@ -167,6 +167,81 @@ bash tools/run/stop_cocktail_motion_preview.sh `run_emulator`, `DRCF M0609`, 관련 RViz까지 확인합니다. 남은 virtual/emulator가 있으면 실제 one-click은 계속 거부됩니다. +### 측정 조인트 티칭값 기반 RViz preview + +`calibration.yaml`의 `cup_pre_place_joints_deg`, `cup_place_joints_deg`, +`press_pre_joints_deg`, `press_contact_joints_deg`를 읽어서 `/joint_states`만 +publish합니다. 실제 로봇 서비스, MoveJoint, MoveLine, 그리퍼 서비스는 호출하지 +않습니다. + +RViz까지 한 번에 띄우려면 이 wrapper를 씁니다. + +```bash +source /opt/ros/humble/setup.bash +cd /home/ssu/Azas +source install/setup.bash +bash tools/run/show_measured_recipe_joint_preview_rviz.sh --dispenser-ids 1 --loop +``` + +이 wrapper는 기본적으로 창현 side-grip 로직과 같은 source tree의 +`src/azas_bringup/config/safety.yaml`, +`src/azas_bringup/config/measured_dispenser_collision.yaml`을 명시적으로 로드합니다. +`side_grip_table`, `side_grip_workspace_*` 안전영역, measured dispenser collision, +full collision YAML marker를 RViz에 띄웁니다. 깜빡임이 있으면 남은 preview 노드를 +정리하고 다시 실행합니다. + +```bash +cd /home/ssu/Azas +source /opt/ros/humble/setup.bash +source install/setup.bash +pkill -f 'preview_measured_dispenser_recipe_rviz.py|publish_measured_recipe_joint_rviz_preview.py|joint_state_relay.py|dispenser_sequence_preview_node|workspace_collision_scene_node|measured_dispenser_collision_scene_node|collision_scene_rviz_publisher|link6_gripper_collision_node|__node:=m0609_robot_state_publisher' || true +pkill -x rviz2 || true + +DISPLAY=:20.0 \ +SHOW_WORKSPACE_SAFETY=true \ +SHOW_MEASURED_DISPENSER_COLLISION=true \ +SHOW_FULL_COLLISION_SCENE=true \ +PUBLISH_WORKSPACE_COLLISION_OBJECTS=true \ +PUBLISH_DISPENSER_COLLISION_OBJECTS=true \ +JOINT_VELOCITY_DEG_S=40.0 \ +HOLD_SECONDS=1.0 \ +PUBLISH_RATE=60.0 \ +bash tools/run/show_measured_recipe_joint_preview_rviz.sh \ + --dispenser-ids 1 \ + --no-press-reset-before-press \ + --press-use-recorded-pre-joints \ + --press-depth-m 0.040 \ + --press-extra-depth-m 0.0 \ + --safe-lift-joint-fallback \ + --press-lock-contact-joints 6 \ + --loop +``` + +첫 번째 터미널에서 RViz demo를 실행합니다. + +```bash +source /opt/ros/humble/setup.bash +cd /home/ssu/Azas +source install/setup.bash +ros2 launch dsr_moveit_config_m0609 demo.launch.py +``` + +다른 터미널에서 측정 조인트 preview publisher를 실행합니다. 이 Python 명령만으로는 +RViz 창을 띄우지 않습니다. + +```bash +source /opt/ros/humble/setup.bash +cd /home/ssu/Azas +source install/setup.bash +python3 tools/run/preview_measured_dispenser_recipe_rviz.py --dispenser-ids 1 --loop +``` + +예: 1번 디스펜서를 2회, 3번 디스펜서를 1회 순서로 표시 + +```bash +python3 tools/run/preview_measured_dispenser_recipe_rviz.py --dispenser-ids 1x2,3 +``` + ### 색상 스캔 자세 RViz preview 디스펜서 색상 구분 전에 쓰는 카메라 보기 관절 자세 diff --git a/asdfas b/asdfas new file mode 100644 index 0000000..5d57cf3 --- /dev/null +++ b/asdfas @@ -0,0 +1,129 @@ +diff --git a/tools/run/check_one_click_cocktail_config.sh b/tools/run/check_one_click_cocktail_config.sh +index 34ac73a..e45cfba 100755 +--- a/tools/run/check_one_click_cocktail_config.sh ++++ b/tools/run/check_one_click_cocktail_config.sh +@@ -89,6 +89,7 @@ try: + raise ValueError(f"dispenser_outlets.{dispenser_id} missing in {calibration_path}") + require_list(outlet, "press_pose_xyz_m", 3, f"dispenser_outlets.{dispenser_id}") + require_list(outlet, "press_pose_rpy_deg", 3, f"dispenser_outlets.{dispenser_id}") ++ require_list(outlet, "press_pre_joints_deg", 6, f"dispenser_outlets.{dispenser_id}") + require_list(outlet, "press_contact_joints_deg", 6, f"dispenser_outlets.{dispenser_id}") +  + grouped: list[tuple[str, int]] = [] +diff --git a/tools/run/check_one_click_cocktail_result.sh b/tools/run/check_one_click_cocktail_result.sh +index 030d95c..51bc4a9 100755 +--- a/tools/run/check_one_click_cocktail_result.sh ++++ b/tools/run/check_one_click_cocktail_result.sh +@@ -52,8 +52,10 @@ fi + for needle in \ + 'RG2 full-open release complete; continuing only after open settle wait' \ + 'RG2 close empty gripper for dispenser press' \ +- 'move to measured press contact joints exactly' \ +- 'press dispenser pump' \ ++ 'PRESS_PRE measured press pre-contact joints' \ ++ 'PRESS_CONTACT measured contact joints' \ ++ 'PRESS_Z_OVERDRIVE from measured contact' \ ++ 'return to PRESS_CONTACT after Z overdrive' \ + 'RG2 soft side-grasp' \ + 'post-grasp lift'; do + if grep -q "${needle}" "${INTEGRATED_LOG}"; then +diff --git a/tools/run/run_color_recipe_sequence.py b/tools/run/run_color_recipe_sequence.py +index 6d17c7f..62085f6 100644 +--- a/tools/run/run_color_recipe_sequence.py ++++ b/tools/run/run_color_recipe_sequence.py +@@ -267,10 +267,10 @@ def main() -> int: + ) + parser.add_argument( + "--press-lock-contact-joints", +- default="6", ++ default="", + help=( +- "measured sequence로 전달할 contact 조인트 잠금 축. 기본 6: " +- "호환 모드에서만 pre 자세 값으로 J6을 유지합니다. 기본 contact-only 경로에서는 사용하지 않습니다." ++ "measured sequence로 전달할 contact 조인트 잠금 축. 기본 빈 값: " ++ "측정된 PRESS_CONTACT joint를 그대로 사용합니다." + ), + ) + parser.add_argument("--press-pre-lift-m", default="0.080") +diff --git a/tools/run/run_measured_dispenser_recipe_sequence.py b/tools/run/run_measured_dispenser_recipe_sequence.py +index 8753107..05e0dd7 100755 +--- a/tools/run/run_measured_dispenser_recipe_sequence.py ++++ b/tools/run/run_measured_dispenser_recipe_sequence.py +@@ -2,8 +2,8 @@ + """Run an ordered measured-dispenser recipe loop. +  + For each dispenser ID this composes measured rule-based primitives: +- move/release cup at DISP_PRE/DISP_PLACE -> measured PRESS_CONTACT FK +- -> generated Z-only PRESS_PRE -> measured contact -> Z-only pump stroke ++ move/release cup at DISP_PRE/DISP_PLACE -> measured PRESS_PRE/PRESS_CONTACT ++ -> Z-only pump overdrive -> measured PRESS_CONTACT -> measured PRESS_PRE + -> re-grasp/lift cup at DISP_PLACE. +  + All cup/dispenser positions come from measured calibration data. This runner +@@ -389,18 +389,26 @@ def print_dry_run_group_detail(args: argparse.Namespace, dispenser_id: str, pres + cup_pre = load_cup_pre_place_joints_deg(dispenser_id) + cup_place = load_cup_place_joints_deg(dispenser_id) + if cup_pre is not None and cup_place is not None: +- print( +- f"[PLAN] dispenser {dispenser_id}: cup DISP_PRE={format_joints_deg(cup_pre)} " +- f"-> DISP_PLACE={format_joints_deg(cup_place)}" +- ) ++ print(f"[Azas] DISP_PRE={format_joints_deg(cup_pre)}") ++ print(f"[Azas] DISP_PLACE={format_joints_deg(cup_place)}") ++ print(f"[PLAN] dispenser {dispenser_id}: cup DISP_PRE -> DISP_PLACE -> RELEASE") + else: + print(f"[PLAN] dispenser {dispenser_id}: cup placement fallback=front_hold_poses") +  + press_contact = load_press_ready_joints_deg(dispenser_id) ++ press_pre = load_press_pre_joints_deg(dispenser_id) if press_contact is not None else None + z_overdrive_mm = (max(args.press_depth_m, 0.0) + max(args.press_extra_depth_m, 0.0)) * 1000.0 +- if press_contact is not None: ++ if press_contact is not None and press_pre is not None: ++ print(f"[Azas] PRESS_PRE={format_joints_deg(press_pre)}") ++ print(f"[Azas] PRESS_CONTACT={format_joints_deg(press_contact)}") ++ print( ++ f"[PLAN] dispenser {dispenser_id}: press PRESS_PRE -> PRESS_CONTACT " ++ f"-> PRESS_Z_OVERDRIVE {z_overdrive_mm:.1f}mm -> PRESS_CONTACT -> PRESS_PRE x{press_count}" ++ ) ++ elif press_contact is not None: ++ print(f"[Azas] PRESS_CONTACT={format_joints_deg(press_contact)}") + print( +- f"[PLAN] dispenser {dispenser_id}: press CONTACT={format_joints_deg(press_contact)} " ++ f"[PLAN] dispenser {dispenser_id}: press contact-only fallback CONTACT " + f"-> generated PRE=CONTACT+Z{max(args.press_contact_entry_lift_m, 0.0) * 1000.0:.1f}mm " + f"-> PRESS=CONTACT-Z{z_overdrive_mm:.1f}mm x{press_count}" + ) +@@ -1297,11 +1305,48 @@ class IntegratedRecipeMotion: + time.sleep(settle_sec) +  + def move_and_release(self, dispenser_id: str) -> None: ++ cup_pre_joints = load_cup_pre_place_joints_deg(dispenser_id) + cup_place_joints = load_cup_place_joints_deg(dispenser_id) +- if cup_place_joints is not None: ++ if cup_pre_joints is not None and cup_place_joints is not None: ++ print(f"[Azas] DISP_PRE={format_joints_deg(cup_pre_joints)}") ++ print(f"[Azas] DISP_PLACE={format_joints_deg(cup_place_joints)}") + print( + f"[Azas] cup placement: dispenser={dispenser_id} using measured " +- "DISP_PLACE with generated high entry; not front_hold_poses" ++ "DISP_PRE -> DISP_PLACE joint teaching; not front_hold_poses" ++ ) ++ cup_pre_joints = self.normalize_joints_near_current( ++ cup_pre_joints, ++ label=f"move to measured DISP{dispenser_id}_PRE cup-place joints", ++ ) ++ self.movej( ++ cup_pre_joints, ++ label=f"DISP_PRE measured DISP{dispenser_id} cup-place approach", ++ velocity=self.args.move_prehold_velocity, ++ acceleration=self.args.move_prehold_acceleration, ++ ) ++ cup_place_joints = self.normalize_joints_near_current( ++ cup_place_joints, ++ label=f"move to measured DISP{dispenser_id}_PLACE cup-place joints", ++ ) ++ self.movej( ++ cup_place_joints, ++ label=f"DISP_PLACE measured DISP{dispenser_id} cup release", ++ velocity=self.args.move_velocity, ++ accelera \ No newline at end of file diff --git a/docs/robot_pipeline_control.html b/docs/robot_pipeline_control.html index a43f1b3..003b3e4 100644 --- a/docs/robot_pipeline_control.html +++ b/docs/robot_pipeline_control.html @@ -841,31 +841,23 @@

Azas Robot Pipeline Control

-
실험 순서: 연결 준비1 창현 side-grip → 성공 확인 후 2 소명 누운 컵 → 성공 확인 후 3 강개발자 뚜껑. 로직 버튼은 한 번에 묶지 말고 단독 실행합니다.
+
현재 후보 순서: tmux 연결 준비수빈 STT/주문 UI색상 스캔창현 side-grip / 소명 누운컵(현재 점검 필요)통합 디스펜서강모 ArUco 뚜껑 체결쉐이킹.
- - - - - - - - - + + + + + - -
-
- - +
@@ -891,7 +883,10 @@

Azas Robot Pipeline Control

+