CustomPickPlaceController는 다음과 같은 로직을 갖는다.

쉽게 말해

• 0 1 2 3 은 잡기
• 4는 이동
• 5 6 7 8 은 놓기
• 9는 다시 제자리

    - Phase 0: Move end_effector above the cube center at the 'end_effector_initial_height'.
    - Phase 1: Lower end_effector down to encircle the target cube
    - Phase 2: Wait for Robot's inertia to settle.
    - Phase 3: close grip.
    - Phase 4: Move end_effector up again, keeping the grip tight (lifting the block).
    - Phase 5: Smoothly move the end_effector toward the goal xy, keeping the height constant.
    - Phase 6: Move end_effector vertically toward goal height at the 'end_effector_initial_height'.
    - Phase 7: loosen the grip.
    - Phase 8: Move end_effector vertically up again at the 'end_effector_initial_height'
    - Phase 9: Move end_effector towards the old xy position.

event_dt에 따라 각 과정을 하나씩 순서대로 실행하게 된다.

사실 코드 메인문에서는 이 과정을 전혀 몰라도 된다. 우리가 맞춰주어야 하는 것은 아래와 같은 각 offset들이다.

        self._pp_single_motion_dict = {
            "end_effector_offset": np.array([0, -0.0, 0]),
            "object_name": "Glass_Cup_1", 
            "picking_offset": np.array([0.1, 0.0, 0.0]),
            "picking_object_offset": np.array([0.05, 0.0, 0.0]),
            "picking_orientation": euler_angles_to_quat(np.array([np.pi/2, np.pi/2, 0 ])),
            
            "placing_position": np.array([0.2, 0.0, 0.6]),
            "placing_offset": np.array([0.0, 0.0, 0.0]),
            "placing_object_offset": np.array([0.0, 0.0, 0]),
            "placing_orientation": euler_angles_to_quat(np.array([0, np.pi/2, 0 ])),
        }

end_effector_offset : 마지막 link > EE 사이의 offset인데 사실 지금은 의미가 없다. 이 좌표가 world 좌표라 로봇이 90도 돌아가거나 잡는 위치와 놓는 위치의 각도가 다르면 쓸모 없어짐

object_name : 잡을 물체의 이름 실제로는 각 환경별 접미사가 추가된다.

Glass_Cup_1_0

Glass_Cup_1_1 이렇게

picking_offset : 물체를 잡기 전 이 위치만큼 더해진 위치에서 그리퍼가 대기한다. 이것도 world 좌표

picking_object_offset : 물체의 위치 + picking_object_offset이 실제 그리퍼가 물건을 잡는 위치이다. end_effector_offset이 의미가 없기 때문에 사실상 지금 이것을 gripper offset으로 사용하고 있다.

picking_orientation : 물체를 잡을 시의 각도, 이건 local 좌표를 사용하도록 코드를 수정해 두었다.