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
------------
h1. Pick and Place with Tactile
h2. 1. Purpose
This document explains how the "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:400px}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