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>
}} 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
{{video("mpro_robot_ctrl_w_taxel_behaviors.mp4",800,400)}}