Finite State Machine

[anhtuduong]

https://github.com/pytransitions/transitions

A possible FSM could include the following states and transitions:

Initial state: This is the starting state of the FSM. It could transition to the following states:

• Object detection state: This state is triggered when the robot detects a LEGO block. It transitions to the object localization state.
• Idle state: This state is triggered when the robot doesn't detect any LEGO blocks for a certain amount of time. It transitions back to the initial state.

Object localization state: This state is triggered when the robot has detected a LEGO block and needs to locate it in 3D space using stereo vision and template matching. It could transition to the following states:

• Pick-up state: This state is triggered when the robot has successfully located the LEGO block and is ready to pick it up with the robotic arm. It transitions to the gripping state.
• Failed localization state: This state is triggered when the robot is unable to accurately locate the LEGO block. It transitions back to the initial state.

Gripping state: This state is triggered when the robot has successfully located the LEGO block and is ready to pick it up with the robotic arm. It could transition to the following states:

• Moving state: This state is triggered when the robot has successfully gripped the LEGO block and needs to move it to a designated location. It transitions to the placing state.
• Failed gripping state: This state is triggered when the robot is unable to successfully grip the LEGO block. It transitions back to the initial state.

Placing state: This state is triggered when the robot has successfully moved the LEGO block to the designated location and needs to release it. It transitions to the initial state.

[anhtuduong]

Initialization State:

• Perform system initialization tasks, such as connecting to the robot arm, camera, and other peripherals.
• Load the trained YOLOv5 model for LEGO block detection.
• Set up initial variables and data structures.

Camera Perception State:

• Capture an image or video stream from the ZED camera.
• Process the image using the YOLOv5 model to detect LEGO blocks.
• Store the detected object positions and orientations.

Object Selection State:

• Analyze the detected LEGO blocks to determine which ones are suitable for building the castle.
• Select the next LEGO block to be picked and placed.

Pick-and-Place State:

• Use the UR5 robot arm to pick up the selected LEGO block.
• Calculate the desired position and orientation for placing the LEGO block based on the existing structure or predefined plan.
• Command the robot arm to move and place the LEGO block in the designated location.

Update State:

• Update the 3D model of the castle structure based on the newly placed LEGO block.
• Update the perception data and the state of the LEGO blocks on the table.
• Check for completion conditions or desired progress milestones.

Check Completion State:

• Check if the castle or desired structure has been completed based on predefined criteria.
• If the structure is complete, transition to a termination state.
• If not, transition back to the Object Selection State or an appropriate state based on the current progress.

Termination State:

• Perform any necessary clean-up tasks.
• Terminate the system or wait for further user instructions.