Pick and place mock up state machine

[ashleshp]

Create a mock up state machine that mocks the current fsm and extends to pick and place task. Traffic light example SM should be referred.

[ashleshp]

Hello @vamsikalagaturu
Could you please review my task , I have implemented the changes in below mentioned branch:
https://github.com/Robots4Sustainability/Finite-State-Machine/tree/dsl/pick_place_fsm

[bhunecke]

@vamsikalagaturu just to confirm: should the state machine and the coordiantion be implemented separately from the eddie interface? So keep the repos separated as it is currently?

[vamsikalagaturu]

@vamsikalagaturu just to confirm: should the state machine and the coordiantion be implemented separately from the eddie interface? So keep the repos separated as it is currently?

• The FSM inside eddie_ros is only the control of the eddie (arms and base)
• Task coordination should happen in a different FSM. So, different repos is fine.

[vamsikalagaturu]

Hello @vamsikalagaturu Could you please review my task , I have implemented the changes in below mentioned branch: https://github.com/Robots4Sustainability/Finite-State-Machine/tree/dsl/pick_place_fsm

• The implementation in both branches is wrong.
• There shouldn't be that many statues.
  • move_ee_cartesian should be the state for example and waiting and result are events or userdata.
• You need to basically rewrite from beginning.

[ashleshp]

Hello @vamsikalagaturu
From my understanding of the review comment , I have done the changes as below. Now we have a single state per action (S_MOVE_TO_APPROACH, S_MOVE_TO_GRASP, S_CLOSE_GRIPPER, S_MOVE_TO_PLACE, S_OPEN_GRIPPER) with success/failure captured as events (E_APPROACH_DONE_OK/FAIL), and the userdata layer handles the pending action context. All “waiting” logic was removed from the state list, so transitions proceed only on those result events,
matching the move_ee_cartesian-as-state guidance while preserving the existing runtime behavior
Could you please verify the updated FSM I have also pushed the code to the branch:
https://github.com/Robots4Sustainability/Finite-State-Machine/tree/dsl/pick_place_fsm

NAME: pick_place

STATES:
S_IDLE,
S_MOVE_TO_APPROACH,
S_MOVE_TO_GRASP,
S_CLOSE_GRIPPER,
S_MOVE_TO_PLACE,
S_OPEN_GRIPPER,
S_FINISHED,
S_ABORT,
S_EXIT

START_STATE: @S_IDLE
CURRENT_STATE: @S_IDLE
END_STATE: @S_EXIT

EVENTS:
E_PERCEPTION_POSE,
E_APPROACH_DONE_OK,
E_APPROACH_DONE_FAIL,
E_GRASP_DONE_OK,
E_GRASP_DONE_FAIL,
E_GRIPPER_CLOSE_DONE_OK,
E_GRIPPER_CLOSE_DONE_FAIL,
E_PLACE_DONE_OK,
E_PLACE_DONE_FAIL,
E_OPEN_DONE_OK,
E_OPEN_DONE_FAIL,
E_RESET

TRANSITIONS:
T_IDLE_MOVE_TO_APPROACH:
FROM: @S_IDLE
TO: @S_MOVE_TO_APPROACH

T_APPROACH_MOVE_TO_GRASP:
    FROM: @S_MOVE_TO_APPROACH
    TO: @S_MOVE_TO_GRASP
T_APPROACH_ABORT:
    FROM: @S_MOVE_TO_APPROACH
    TO: @S_ABORT


T_GRASP_CLOSE_GRIPPER:
    FROM: @S_MOVE_TO_GRASP
    TO: @S_CLOSE_GRIPPER
T_GRASP_ABORT:
    FROM: @S_MOVE_TO_GRASP
    TO: @S_ABORT


T_CLOSE_GRIPPER_MOVE_TO_PLACE:
    FROM: @S_CLOSE_GRIPPER
    TO: @S_MOVE_TO_PLACE
T_CLOSE_GRIPPER_ABORT:
    FROM: @S_CLOSE_GRIPPER
    TO: @S_ABORT


T_PLACE_OPEN_GRIPPER:
    FROM: @S_MOVE_TO_PLACE
    TO: @S_OPEN_GRIPPER
T_PLACE_ABORT:
    FROM: @S_MOVE_TO_PLACE
    TO: @S_ABORT


T_OPEN_FINISHED:
    FROM: @S_OPEN_GRIPPER
    TO: @S_FINISHED
T_OPEN_ABORT:
    FROM: @S_OPEN_GRIPPER
    TO: @S_ABORT


T_FINISHED_IDLE:
    FROM: @S_FINISHED
    TO: @S_IDLE
T_ABORT_IDLE:
    FROM: @S_ABORT
    TO: @S_IDLE

REACTIONS:

R_E_PERCEPTION_POSE:
    WHEN: @E_PERCEPTION_POSE
    DO: @T_IDLE_MOVE_TO_APPROACH


R_E_APPROACH_DONE_OK:
    WHEN: @E_APPROACH_DONE_OK
    DO: @T_APPROACH_MOVE_TO_GRASP
R_E_APPROACH_DONE_FAIL:
    WHEN: @E_APPROACH_DONE_FAIL
    DO: @T_APPROACH_ABORT


R_E_GRASP_DONE_OK:
    WHEN: @E_GRASP_DONE_OK
    DO: @T_GRASP_CLOSE_GRIPPER
R_E_GRASP_DONE_FAIL:
    WHEN: @E_GRASP_DONE_FAIL
    DO: @T_GRASP_ABORT


R_E_GRIPPER_CLOSE_DONE_OK:
    WHEN: @E_GRIPPER_CLOSE_DONE_OK
    DO: @T_CLOSE_GRIPPER_MOVE_TO_PLACE
R_E_GRIPPER_CLOSE_DONE_FAIL:
    WHEN: @E_GRIPPER_CLOSE_DONE_FAIL
    DO: @T_CLOSE_GRIPPER_ABORT

R_E_PLACE_DONE_OK:
    WHEN: @E_PLACE_DONE_OK
    DO: @T_PLACE_OPEN_GRIPPER
R_E_PLACE_DONE_FAIL:
    WHEN: @E_PLACE_DONE_FAIL
    DO: @T_PLACE_ABORT

R_E_OPEN_DONE_OK:
    WHEN: @E_OPEN_DONE_OK
    DO: @T_OPEN_FINISHED
R_E_OPEN_DONE_FAIL:
    WHEN: @E_OPEN_DONE_FAIL
    DO: @T_OPEN_ABORT


R_E_RESET_FINISHED:
    WHEN: @E_RESET
    DO: @T_FINISHED_IDLE
R_E_RESET_ABORT:
    WHEN: @E_RESET
    DO: @T_ABORT_IDLE

[vamsikalagaturu]

@ashleshp please refrain from pasting complete code in the discussions. Use reference links by Github to refer to piece of code you want to. And if you need a code review, you need to create a PR first. Otherwise, its hard to track changes. This applies to everyone.

Regarding the FSM,

• S_MOVE_TO_APPROACH and S_MOVE_TO_PLACE can be merged into S_MOVE_EE_CARTESIAN
• Gripper states (open and close) can be merged into one
• What is the requirement for S_ABORT and S_FINISHED -> S_EXIT should be doing those jobs.

Integrate with the complete pick-place pipeline, then I can review the code.