기존 로직
- 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.
바뀐 로직
6 : 높이를 insertion position에 맞춤과 동시에 placing offset 만큼 멀찍이 떨어지기
7 : port position에 Insertion 높이는 유지
8 : loose grip
9 : 다시 placing offset 만큼 떨어지기, 위치는 유지
10 : Move end_effector towards the old xy position
elif self._event == 6:
position_target = np.array(
[
interpolated_xy[0] + end_effector_offset[0] + placing_offset[0],
interpolated_xy[1] + end_effector_offset[1] + placing_offset[1],
placing_position[2] + end_effector_offset[2] + placing_offset[2],
]
)
elif self._event == 7:
position_target = np.array(
[
interpolated_xy[0] + end_effector_offset[0],
interpolated_xy[1] + end_effector_offset[1],
placing_position[2] + end_effector_offset[2],
]
)
elif self._event == 9:
position_target = np.array(
[
interpolated_xy[0] + end_effector_offset[0] + placing_offset[0],
interpolated_xy[1] + end_effector_offset[1] + placing_offset[1],
placing_position[2] + end_effector_offset[2] + placing_offset[2],
]
)
elif self._event == 10:
position_target = np.array(
[
interpolated_xy[0] + end_effector_offset[0] + placing_offset[0],
interpolated_xy[1] + end_effector_offset[1] + placing_offset[1],
# self._h0,
placing_position[2] + end_effector_offset[2],
]
)
simplescreenrecorder-2024-07-14_14.20.29.mp4
Insertion Env / Controller 패키지 새로 만듦