System Implementation

Extraction Subsystem

The Extraction Subsystem comprises two primary components: the Gripper Mechanism and the Cutting Mechanism, each designed with specific functionality to support the extraction process.

Gripper Mechanism

The Gripper Mechanism features a tridigital design with three soft-bodied fingers. These fingers operate via a tendon-driven system connected to Dynamixel motors that provide precise control over gripping pressure and position. The flexible nature of the fingers enables adaptive grasping of irregular objects.

Cutting Mechanism

The Cutting Mechanism employs a similar tendon-based design principle. It utilizes a single Dynamixel motor that actuates a tendon system, which in turn rotates a specialized blade. This blade delivers shear forces to effectively sever the peduncle during the extraction process.

Control Implementation

The system architecture incorporates an end-effector node that instantiates separate driver modules for each mechanism. This modular approach enables independent control of the gripper and cutter components. The control system:

• Monitors incoming command messages for both mechanisms
• Processes normalized operational parameters (0-100% range)
• Employs transfer functions to convert these normalized values into precise joint angles
• Receives dedicated GripperCommand and CutterCommand message types through separate channels

This implementation ensures precise coordination between gripping and cutting operations during the extraction sequence.

Perception Subsystem

The Perception Subsystem executes five critical functions within the operational framework: pepper segmentation, coarse and fine pose estimation, peduncle segmentation, and peduncle point-of-interaction (POI) determination.

Image Acquisition and Pepper Segmentation

The system employs a Realsense D405 camera for primary image acquisition. Pepper identification utilizes a YOLOv8 instance segmentation model that effectively discriminates pepper objects from environmental elements. Following segmentation, the system identifies the proximally closest pepper specimen, extracts its mask, and correlates it with depth imaging to generate a three-dimensional point cloud representation. This process facilitates accurate centroid calculation of the target specimen.

Pepper Pose Estimation

The pose estimation procedure operates in two sequential phases:

1. Coarse Pose Estimation:
   • Leverages the YOLOv8-generated segmentation masks to isolate the pepper fruit point cloud
   • Calculates the geometric centroid of the point cloud as the initial positional estimate
   • Publishes this data via a unified ROS message format that encapsulates both positional and dimensional parameters
   • Defines the pepper’s volumetric properties using cylindrical primitive representations
   • Integrates both fruit and peduncle morphological data within a consolidated message structure
2. Fine Pose Estimation:
   • Activation occurs upon successful navigation to the pre-grasp position
   • Provides high-precision spatial coordinates essential for grasp planning and execution
   • Serves as the deterministic input for final manipulation operations

Peduncle Analysis and Interaction Point Determination

The peduncle segmentation module implements a YOLOv8 model specifically optimized for stem structure identification. The system extracts the medial axis of the segmented peduncle utilizing the Skimage Python library’s morphological functions. This axis is subsequently modeled through parabolic curve fitting.

The orientation determination employs least squares error analysis between the extracted medial axis points and the fitted parabolic curve to ascertain optimal vertical or horizontal alignment. The critical point-of-interaction for the cutting operation is algorithmically established at a position equivalent to half the total curve length, measured from the superior aspect of the pepper fruit.

Planning Subsystem

The Planning Subsystem orchestrates trajectory generation and execution for robotic arm movement during the pepper extraction process. This component interfaces with perception data to establish optimal motion pathways while maintaining system safety through comprehensive collision management protocols.

Dual-Arm Collision Avoidance Framework

The system implements a sophisticated collision avoidance methodology to address the constrained operational environment where arms operate in proximity to agricultural structures, produce, and each other. The architecture segregates manipulators into discrete move groups and employs a specialized subroutine that:

1. Identifies the currently operational move group
2. Generates conservative collision boundary objects encapsulating the non-operational move group
3. Integrates with the MoveIt API to prevent trajectory plans that would result in inter-arm collisions
4. Automatically terminates planning operations when target positions interfere with established collision boundaries

This approach enables simultaneous operation of dual manipulators within shared workspace environments while maintaining system integrity.

Grasp Pose Planning

When the system receives pepper position data from the perception subsystem, it applies a critical transformation to accommodate the specialized grasping requirements. The default orientation alignment between the end-effector’s z-axis and the pepper’s vertical axis is unsuitable for lateral grasping. The system addresses this through:

• Application of a static transform that rotates the target pose orientation by 90 degrees around the target’s x-axis
• Forwarding of this modified orientation to the MoveIt planner for trajectory calculation
• Retention of the original orientation parameters for visualization purposes in the simulation environment
• Strategic offset positioning of the target cylinder to optimize planning operations

Execution Sequence

The system workflow consists of a sequential process wherein pepper pose data received from the perception node undergoes transformation for grasp planning. The planning node:

1. Spawns representative cylindrical objects at points of interest within the simulation environment
2. Calculates optimal path trajectories to these targets (visualized as waypoint sequences)
3. Executes the planned motion sequence through the ROSService interface
4. Dynamically manages simulation objects by replacing previous targets when new points of interest are generated

This cyclical operation enables continuous harvesting operations with efficient resource management and accurate spatial representation.

Dual Arm Motion Planning