Task #228
Updated by Sanghoon Lee 27 days ago
Create an objective consisting of Xela 2F behaviors using uSPr2F Taxel inputs. The behaviors for this Objective are: 1. Open Gripper 2. Pick Sequence 3. Transport with Slip Monitoring 4. Place 5. Retreat !{width:800px}clipboard-202603071101-xhzmk.png! ------------ h1. Pick and Place with Tactile h2. 1. Purpose This document explains how the "pick_and_place_with_tactile.xml"{{collapse <pre><code class="xml"> <?xml version="1.0" encoding="UTF-8" ?> <root BTCPP_format="4" main_tree_to_execute="Pick and Place with Tactile"> <BehaviorTree ID="Pick and Place with Tactile" _description="Open the gripper, move to the pick pose, compute a tactile baseline, grasp with tactile stop, transport with slip monitoring, release at the place pose, and retreat." _favorite="false"> <Control ID="Sequence" name="Pick and Place Sequence"> <Action ID="SetTelemetryPhase" name="Phase Start" phase="OBJECTIVE_START" note="Pick and Place objective started"/> <!-- 1. Open Gripper --> <Action ID="SetTelemetryPhase" name="Phase Initial Open" phase="INITIAL_OPEN" note="Initial gripper open"/> <Action ID="RapidOpen" name="Initial Open" open_speed="1.0" /> <!-- 2. Pick Sequence --> <Action ID="SetTelemetryPhase" name="Phase Move Pick Zone" phase="MOVE_TO_PICK_ZONE" note="Move to look-at waypoint"/> <SubTree ID="Move to Waypoint" _collapsed="true" joint_group_name="manipulator" waypoint_name="Look at Pick and Place Zone" controller_names="joint_trajectory_controller" /> <Action ID="SetTelemetryPhase" name="Phase Move Pick" phase="MOVE_TO_PICK" note="Move to pick waypoint"/> <SubTree ID="Move to Waypoint" _collapsed="true" joint_group_name="manipulator" waypoint_name="Pick Block" controller_names="joint_trajectory_controller" /> <Action ID="SetTelemetryPhase" name="Phase Baseline" phase="BASELINE" note="Compute tactile baseline"/> <Action ID="ComputeTactileBaseline" name="Compute Tactile Baseline" topic_name="/x_taxel_2f" baseline_key="pick_grasp_baseline" model_name="uSPa46" sample_duration="0.35" max_wait_duration="1.5" min_samples="4"/> <Action ID="SetTelemetryPhase" name="Phase Grasp" phase="GRASP_CLOSE" note="Slow close with tactile stop"/> <!-- <Action ID="GraspWithForceStop" name="Grasp With Force Stop" target_force="2.0" target_single_force="0.5" target_taxel_delta="0.06" target_taxel_delta_topk_sum="0.10" target_taxel_sigma="4.0" active_taxel_delta="0.006" sparse_max_active_taxels="2" sparse_min_peak_taxel_delta="0.010" sparse_min_peak_taxel_sigma="2.5" sigma_noise_floor="0.003" point_contact_topk_count="4" closure_speed="0.1" stop_wait_duration="0.1" model_name="uSPa46" baseline_duration="0.35" baseline_timeout="1.5" baseline_min_samples="4" point_contact_debounce_ticks="2" use_external_baseline="true" baseline_key="pick_grasp_baseline" baseline_max_age="3.0" fallback_to_internal_baseline="true" topic_name="/x_taxel_2f" profile_name="general" profile_config_yaml="" debug_contact_values="true" debug_interval_ms="250" debug_top_taxels="3"/> --> <Action ID="GraspWithForceStop" name="Grasp With Force Stop (Crane Sharp Profile)" model_name="uSPa46" profile_name="crane_sharp" profile_config_yaml="" debug_contact_values="true" debug_interval_ms="250" debug_top_taxels="3"/> <!-- 3. Transport with Slip Monitoring --> <Action ID="SetTelemetryPhase" name="Phase Transport" phase="TRANSPORT_WITH_SLIP_MONITOR" note="Move to place with slip monitoring"/> <Control ID="Parallel" success_count="1" failure_count="1"> <SubTree ID="Move to Waypoint" _collapsed="true" waypoint_name="Place Block" joint_group_name="manipulator" controller_names="joint_trajectory_controller" /> <Action ID="SlipMonitor" name="Slip Monitor" profile_name="crane_sharp" profile_config_yaml=""/> <!-- <Action ID="SlipMonitor" name="Slip Monitor (Origami Profile)" topic_name="/x_taxel_2f" close_pos_topic="/x_telemetry_2f/grasp_metric/close_pos" profile_name="origami" profile_config_yaml=""/> --> </Control> <Action ID="SlipMonitor" name="Slip Monitor Hold Confirm" profile_name="crane_sharp" profile_config_yaml="" monitor_duration="0.20"/> <!-- 4. Place --> <Action ID="SetTelemetryPhase" name="Phase Release" phase="RELEASE" note="Open gripper at place pose"/> <Action ID="RapidOpen" name="Place Release" open_speed="1.0"/> <!-- 5. Retreat --> <Action ID="SetTelemetryPhase" name="Phase Retreat" phase="RETREAT" note="Move back to look-at waypoint"/> <SubTree ID="Move to Waypoint" _collapsed="true" waypoint_name="Look at Pick and Place Zone" joint_group_name="manipulator" controller_names="joint_trajectory_controller" /> <Action ID="SetTelemetryPhase" name="Phase Complete" phase="OBJECTIVE_COMPLETE" note="Pick and Place objective complete"/> </Control> </BehaviorTree> <TreeNodesModel> <SubTree ID="Pick and Place with Tactile"> <MetadataFields> <Metadata runnable="true" /> <Metadata subcategory="Tactile Behaviors" /> </MetadataFields> </SubTree> <Action ID="ComputeTactileBaseline"> <input_port name="topic_name" default="/x_taxel_2f" type="std::string">Tactile sensor topic</input_port> <input_port name="baseline_key" default="tactile_baseline_2f" type="std::string">Baseline storage key</input_port> <input_port name="model_name" default="auto" type="std::string">Target model name (2F model or auto)</input_port> <input_port name="model_config_yaml" default="" type="std::string">Optional path to model config YAML. Empty uses package default.</input_port> <input_port name="sample_duration" default="0.35" type="double">Baseline collection duration before success (seconds)</input_port> <input_port name="max_wait_duration" default="1.5" type="double">Maximum wait for baseline samples (seconds)</input_port> <input_port name="min_samples" default="4" type="int">Minimum tactile samples for baseline</input_port> <MetadataFields> <MetadataField name="subcategory" value="Tactile Behaviors"/> <MetadataField name="description" value="Compute and store tactile baseline for 2F grasp behaviors."/> </MetadataFields> </Action> <Action ID="SetTelemetryPhase"> <input_port name="phase" default="UNKNOWN" type="std::string">Global telemetry phase label</input_port> <input_port name="event" default="phase_changed" type="std::string">Event name published to grasp_event topic</input_port> <input_port name="source" default="objective" type="std::string">Event source label</input_port> <input_port name="note" default="" type="std::string">Optional free-text note</input_port> <MetadataFields> <MetadataField name="subcategory" value="Tactile Behaviors"/> <MetadataField name="description" value="Publish global telemetry phase/event for objective-level tracing."/> </MetadataFields> </Action> <Action ID="GraspWithForceStop"> <input_port name="target_force" default="2.0" type="double">Force threshold to stop grasping (N, Total Sum)</input_port> <input_port name="target_single_force" default="0.5" type="double">Force threshold for single taxel to stop grasping (N, Peak)</input_port> <input_port name="target_taxel_delta" default="0.06" type="double">Threshold for point contact from absolute taxel Z delta (normalized units)</input_port> <input_port name="target_taxel_delta_topk_sum" default="0.10" type="double">Threshold for summed absolute point contact over top-N taxels (normalized units)</input_port> <input_port name="target_taxel_sigma" default="4.0" type="double">Threshold for point contact from per-taxel sigma-normalized delta</input_port> <input_port name="active_taxel_delta" default="0.006" type="double">Delta threshold for counting active taxels</input_port> <input_port name="sparse_max_active_taxels" default="2" type="int">Maximum active taxels for sparse (pointy) contact mode</input_port> <input_port name="sparse_min_peak_taxel_delta" default="0.010" type="double">Minimum peak taxel delta for sparse (pointy) contact mode</input_port> <input_port name="sparse_min_peak_taxel_sigma" default="2.5" type="double">Minimum peak sigma delta for sparse (pointy) contact mode</input_port> <input_port name="sigma_noise_floor" default="0.003" type="double">Noise floor for sigma normalization to avoid divide-by-zero</input_port> <input_port name="point_contact_topk_count" default="4" type="int">Top-N taxels used for summed point contact</input_port> <input_port name="closure_speed" default="0.1" type="double">Normalized close speed (0.0 - 1.0). Lower is slower.</input_port> <input_port name="hold_effort" default="30.0" type="double">Grip effort used to hold the gripper after tactile contact is detected</input_port> <input_port name="hold_close_bias" default="0.003" type="double">Extra close bias added to the latched measured gripper position during hold</input_port> <input_port name="hold_settle_duration" default="0.12" type="double">Time in seconds that gripper position and velocity must remain settled before success</input_port> <input_port name="hold_reassert_period" default="0.05" type="double">Period in seconds for re-sending the hold goal while confirming contact</input_port> <input_port name="hold_position_retreat_tolerance" default="0.002" type="double">Allowed retreat from the latched hold position while confirming contact</input_port> <input_port name="hold_velocity_tolerance" default="0.01" type="double">Maximum allowed measured gripper velocity magnitude while confirming contact</input_port> <input_port name="stop_wait_duration" default="0.1" type="double">Wait time (s) after stopping before returning SUCCESS</input_port> <input_port name="require_dual_contact" default="false" type="bool">Require both 2F tactile modules to remain in contact before success.</input_port> <input_port name="dual_contact_hold_duration" default="0.1" type="double">Time in seconds that dual contact must remain stable before success.</input_port> <input_port name="gripper_joint_state_topic" default="/joint_states" type="std::string">Joint state topic used to latch measured gripper position</input_port> <input_port name="gripper_joint_name" default="finger_joint" type="std::string">Joint name used to track measured gripper position</input_port> <input_port name="model_name" default="auto" type="std::string">Target model name for tactile filtering</input_port> <input_port name="model_config_yaml" default="" type="std::string">Optional path to model config YAML. Empty uses package default.</input_port> <input_port name="baseline_duration" default="0.35" type="double">Baseline collection duration before closing (seconds)</input_port> <input_port name="baseline_timeout" default="1.5" type="double">Maximum wait for baseline; fallback to force-only when exceeded (seconds)</input_port> <input_port name="baseline_min_samples" default="4" type="int">Minimum tactile samples required for baseline</input_port> <input_port name="point_contact_debounce_ticks" default="2" type="int">Consecutive ticks required before point-contact stop</input_port> <input_port name="use_external_baseline" default="false" type="bool">Use baseline from ComputeTactileBaseline behavior</input_port> <input_port name="baseline_key" default="tactile_baseline_2f" type="std::string">External baseline storage key</input_port> <input_port name="baseline_max_age" default="3.0" type="double">Maximum accepted age for external baseline (seconds)</input_port> <input_port name="fallback_to_internal_baseline" default="true" type="bool">Fallback to internal baseline collection when external baseline is unavailable</input_port> <input_port name="topic_name" default="/x_taxel_2f" type="std::string">Tactile sensor topic</input_port> <input_port name="profile_name" default="general" type="std::string">Profile name for grasp parameter set</input_port> <input_port name="profile_config_yaml" default="" type="std::string">Optional path to grasp profile YAML</input_port> <input_port name="debug_contact_values" default="false" type="bool">Print tactile debug values while closing</input_port> <input_port name="debug_interval_ms" default="250" type="int">Throttle interval for tactile debug logging (ms)</input_port> <input_port name="debug_top_taxels" default="3" type="int">Number of top taxel deltas printed in debug logs (1-3)</input_port> <MetadataFields> <MetadataField name="subcategory" value="Tactile Behaviors"/> <MetadataField name="description" value="Slow-close gripper, stop on broad or point tactile contact, or continue after full close if no contact."/> </MetadataFields> </Action> <Action ID="SlipMonitor"> <input_port name="slip_threshold" default="0.5" type="double">Threshold for shear force change</input_port> <input_port name="force_increment" default="10.0" type="double">Effort increment (%) when slip detected</input_port> <input_port name="topic_name" default="/x_taxel_2f" type="std::string">Tactile sensor topic</input_port> <input_port name="close_pos_topic" default="/x_telemetry_2f/grasp_metric/close_pos" type="std::string">Topic for latest close position from grasp telemetry</input_port> <input_port name="slip_mode" default="thick" type="std::string">Slip-monitor mode: sharp, thick, or wide</input_port> <input_port name="position_increment" default="0.01" type="double">Position increment per slip event</input_port> <input_port name="max_position_delta" default="0.10" type="double">Maximum additional closing from entry close position</input_port> <input_port name="max_position_limit" default="0.65" type="double">Maximum close position while slip monitoring</input_port> <input_port name="initial_target_position" default="0.20" type="double">Fallback initial close position</input_port> <input_port name="point_slip_threshold" default="0.01" type="double">Threshold for sharp-contact taxel peak delta</input_port> <input_port name="point_slip_topk_sum_threshold" default="0.02" type="double">Threshold for sharp-contact top-k taxel delta sum</input_port> <input_port name="point_slip_xy_threshold" default="0.006" type="double">Threshold for sharp-contact taxel XY delta</input_port> <input_port name="point_slip_xy_topk_sum_threshold" default="0.015" type="double">Threshold for sharp-contact top-k XY delta sum</input_port> <input_port name="point_slip_active_taxel_delta" default="0.004" type="double">Delta threshold for active sharp-contact taxels</input_port> <input_port name="point_slip_max_active_taxels" default="4" type="int">Maximum active taxels for sharp-contact slip mode</input_port> <input_port name="point_slip_topk_count" default="4" type="int">Top-k taxels used for sharp-contact slip detection</input_port> <input_port name="monitor_duration" default="0.0" type="double">Optional bounded monitoring duration before SUCCESS</input_port> <input_port name="drift_confirm_ticks" default="2" type="int">Consecutive ticks required for sustained side-drift slip</input_port> <input_port name="profile_name" default="general" type="std::string">Profile name for slip-monitor parameter set</input_port> <input_port name="profile_config_yaml" default="" type="std::string">Optional path to grasp profile YAML</input_port> <MetadataFields> <MetadataField name="subcategory" value="Tactile Behaviors"/> <MetadataField name="description" value="Monitors slip and increases gripper effort."/> </MetadataFields> </Action> <Action ID="RapidOpen"> <input_port name="open_speed" default="1.0" type="double">Speed factor for opening (0.0 - 1.0)</input_port> <MetadataFields> <MetadataField name="subcategory" value="Tactile Behaviors"/> <MetadataField name="description" value="Opens gripper rapidly."/> </MetadataFields> </Action> </TreeNodesModel> </root> </code></pre> }} "pick_and_place_with_tactile.xml":/home/invokelee/my_moveit_pro/01_wk_xela_mpro_dev_ws/src/ur5e_x2f_140c_config4/objectives/pick_and_place_with_tactile.xml Objective currently works, step by step and parameter by parameter. It is intended for: * users who want a quick understanding of the Objective flow * developers tuning tactile grasp or slip-monitor behavior * operators who want to correlate Sidecar Analysis events with Objective phases h2. 2. One-Line Summary This Objective: # opens the gripper # moves to the pick pose # computes a tactile baseline # grasps the object with tactile stop logic # monitors slip during transport # releases at the place pose # retreats to a safe observation pose h2. 3. Workflow Diagram <pre> OBJECTIVE_START -> INITIAL_OPEN / RapidOpen -> MOVE_TO_PICK_ZONE / Move to Look at Pick and Place Zone -> MOVE_TO_PICK / Move to Pick Block -> BASELINE / ComputeTactileBaseline -> GRASP_CLOSE / GraspWithForceStop -> dual contact and joint settled? -> no: keep grasp hold-confirm loop -> yes: continue -> TRANSPORT_WITH_SLIP_MONITOR -> Move to Place Block -> SlipMonitor (sharp mode) -> slip detected? -> yes: incremental re-close + publish slip_warning -> no: continue monitoring -> Place hold confirm / SlipMonitor 0.20 s -> RELEASE / RapidOpen -> RETREAT / Move to Look at Pick and Place Zone -> OBJECTIVE_COMPLETE </pre> !{width:500px}clipboard-202603070849-99t5i.png! This diagram reflects the current control flow in the Objective: * the grasp stage does not succeed until dual tactile contact and gripper settling are both confirmed * transport runs arm motion and slip monitoring in parallel * slip detection triggers incremental re-close commands rather than ending the Objective * a short bounded slip check also runs after the place waypoint is reached h2. 4. Full Sequence h3. 4.1. Start and Initial Open * @SetTelemetryPhase(OBJECTIVE_START)@ * @SetTelemetryPhase(INITIAL_OPEN)@ * @RapidOpen@ Meaning: * records the start of the Objective in the telemetry event stream * fully opens the gripper before entering the pick sequence h3. 4.2. Move to Pick * @SetTelemetryPhase(MOVE_TO_PICK_ZONE)@ * @Move to Waypoint("Look at Pick and Place Zone")@ * @SetTelemetryPhase(MOVE_TO_PICK)@ * @Move to Waypoint("Pick Block")@ Meaning: * first moves to a look-at or staging pose * then enters the actual pick pose * this stage is arm motion only; tactile grasping has not started yet h3. 4.3. Compute the Tactile Baseline * @SetTelemetryPhase(BASELINE)@ * @ComputeTactileBaseline@ Current settings: * @topic_name="/x_taxel_2f"@ * @baseline_key="pick_grasp_baseline"@ * @model_name="uSPa46"@ * @sample_duration="0.35"@ Meaning: * stores the no-load taxel state immediately before grasping * allows @GraspWithForceStop@ to compare live tactile data against a fresh baseline * supports both broad contact detection and sparse point-contact detection h3. 4.4. Tactile Grasp * @SetTelemetryPhase(GRASP_CLOSE)@ * @GraspWithForceStop(profile_name="origami", require_dual_contact="true", ...)@ Current behavior: * uses the @origami@ grasp profile * requires both 2F tactile modules to confirm contact before grasp success * does not return success immediately when contact is detected * reads @/joint_states@ and tracks @finger_joint@ * latches the measured gripper position at contact time * repeatedly reasserts a hold goal with a small close bias * waits until the finger joint position and velocity are settled before returning @SUCCESS@ Important grasp parameters in the current XML: * @require_dual_contact="true"@ * @dual_contact_hold_duration="0.10"@ * @hold_effort="45.0"@ * @hold_close_bias="0.006"@ * @hold_settle_duration="0.15"@ * @hold_reassert_period="0.03"@ * @hold_position_retreat_tolerance="0.0015"@ * @hold_velocity_tolerance="0.006"@ Why these values matter: * they prevent success on one-sided contact * they reduce the small release or back-drive effect immediately after contact * they keep the Objective from advancing until the actual gripper joint has settled h3. 4.5. Transport with Slip Monitoring * @SetTelemetryPhase(TRANSPORT_WITH_SLIP_MONITOR)@ * @Parallel@ * @Move to Waypoint("Place Block")@ * @SlipMonitor(slip_mode="sharp", profile_name="origami")@ Meaning: * the arm moves toward the place pose while slip monitoring runs in parallel * the current Crane use case uses @sharp@ mode because contact is concentrated on narrow wing edges Current slip-monitor behavior: * prioritizes sparse point-contact slip detection over broad pad shear * increases the close target incrementally when slip is detected * publishes slip events with @mode@, @reason@, @close_pos@, and @target_effort@ h3. 4.6. Short Hold Confirm at Place * @SlipMonitor(monitor_duration="0.20")@ Meaning: * performs one more bounded slip check immediately after reaching the place waypoint * helps catch late slip that appears at the end of transport or just before release h3. 4.7. Release and Retreat * @SetTelemetryPhase(RELEASE)@ * @RapidOpen@ * @SetTelemetryPhase(RETREAT)@ * @Move to Waypoint("Look at Pick and Place Zone")@ * @SetTelemetryPhase(OBJECTIVE_COMPLETE)@ Meaning: * opens the gripper at the place pose * retreats back to the observation waypoint * records Objective completion in telemetry h2. 5. Behavior Roles h3. 5.1. @SetTelemetryPhase@ * publishes Objective phase labels into the event stream * used by logs and the Sidecar timeline h3. 5.2. @ComputeTactileBaseline@ * stores the tactile reference state before grasping * provides the baseline for point-contact comparison h3. 5.3. @GraspWithForceStop@ * closes slowly and stops based on tactile contact * evaluates both broad force signals and sparse point-contact signals * currently enforces dual contact and measured joint settling h3. 5.4. @SlipMonitor@ * monitors the grasp during transport * supports @sharp@, @thick@, and @wide@ modes * performs incremental re-close behavior and emits warning events on slip h3. 5.5. @RapidOpen@ * simple fast-open action used for initial open and place release h2. 6. Main Topics Used by This Objective * @/x_taxel_2f@ * @/joint_states@ * @/x_telemetry_2f/grasp_metric/close_pos@ * @/x_telemetry_2f/grasp_telemetry@ * @/x_telemetry_2f/grasp_event@ * @/robotiq_gripper_controller/gripper_cmd@ h2. 7. What Appears in Sidecar Analysis When this Objective runs, the Analysis timeline typically shows: * phase markers such as @OBJECTIVE_START@, @INITIAL_OPEN@, @MOVE_TO_PICK_ZONE@, @MOVE_TO_PICK@, @BASELINE@, @GRASP_CLOSE@, @TRANSPORT_WITH_SLIP_MONITOR@, @RELEASE@, @RETREAT@, and @OBJECTIVE_COMPLETE@ * grasp events such as @contact_detected@ * @no_contact_full_close@ for empty or failed grasps * slip events such as @slip_warning@ The current slip tooltip also shows: * @mode@ * @reason@ * @close_pos@ * @target_effort@ h2. 8. Main Tuning Knobs h3. 8.1. Grasp-side tuning * @dual_contact_hold_duration@ * @hold_close_bias@ * @hold_settle_duration@ * @hold_position_retreat_tolerance@ * @hold_velocity_tolerance@ h3. 8.2. Slip-side tuning * @slip_mode@ * @drift_confirm_ticks@ * @position_increment@ * @force_increment@ * @point_slip_*@ h3. 8.3. Controller-side tuning Related file: "ur5e_x2f_140.ros2_control.yaml":/home/invokelee/my_moveit_pro/01_wk_xela_mpro_dev_ws/src/ur5e_x2f_140c_config4/config/control/ur5e_x2f_140.ros2_control.yaml Current key settings: * @stall_timeout@ * @stall_velocity_threshold@ * @goal_tolerance@ h2. 9. Operational Checkpoints * verify that the object is not already touching the gripper during the baseline phase * verify that the finger joint actually settles before arm transport starts * verify that @slip_warning@ appears in Analysis when transport slip occurs * verify that the bounded slip monitor catches late slip near the place pose