52 lines
2.9 KiB
Python
52 lines
2.9 KiB
Python
|
|
from .base_task import Base_task
|
|
from .dual_bottles_pick_easy import dual_bottles_pick_easy
|
|
from .utils import *
|
|
import sapien
|
|
|
|
class gpt_dual_bottles_pick_easy(dual_bottles_pick_easy):
|
|
def play_once(self):
|
|
# Retrieve the actors and their data
|
|
red_bottle = self.actor_name_dic['red_bottle']
|
|
green_bottle = self.actor_name_dic['green_bottle']
|
|
left_target_position = self.actor_name_dic['left_bottle_target_position']
|
|
right_target_position = self.actor_name_dic['right_bottle_target_position']
|
|
|
|
red_bottle_data = self.actor_data_dic['red_bottle_data']
|
|
green_bottle_data = self.actor_data_dic['green_bottle_data']
|
|
left_target_position_data = self.actor_data_dic['left_bottle_target_position']
|
|
right_target_position_data = self.actor_data_dic['right_bottle_target_position']
|
|
|
|
# Get the target positions for the bottles
|
|
left_target_pose = self.get_actor_goal_pose(left_target_position, left_target_position_data, id=0)
|
|
right_target_pose = self.get_actor_goal_pose(right_target_position, right_target_position_data, id=0)
|
|
|
|
# Get the grasp poses for the bottles
|
|
red_pre_grasp_pose = self.get_grasp_pose_to_grasp_object(endpose_tag="left", actor=red_bottle, actor_data=red_bottle_data, pre_dis=0.09)
|
|
red_grasp_pose = self.get_grasp_pose_to_grasp_object(endpose_tag="left", actor=red_bottle, actor_data=red_bottle_data, pre_dis=0)
|
|
|
|
green_pre_grasp_pose = self.get_grasp_pose_to_grasp_object(endpose_tag="right", actor=green_bottle, actor_data=green_bottle_data, pre_dis=0.09)
|
|
green_grasp_pose = self.get_grasp_pose_to_grasp_object(endpose_tag="right", actor=green_bottle, actor_data=green_bottle_data, pre_dis=0)
|
|
|
|
# Move both arms to the pre-grasp positions
|
|
self.together_move_to_pose_with_screw(red_pre_grasp_pose, green_pre_grasp_pose)
|
|
|
|
# Move both arms to the grasp positions
|
|
self.together_move_to_pose_with_screw(red_grasp_pose, green_grasp_pose)
|
|
|
|
# Close both grippers to grasp the bottles
|
|
self.together_close_gripper()
|
|
|
|
# Get the target poses for the bottles at the target positions
|
|
red_target_pose = self.get_grasp_pose_from_goal_point_and_direction(red_bottle, red_bottle_data, endpose_tag="left", actor_functional_point_id=0, target_point=left_target_pose, target_approach_direction=self.world_direction_dic['top_down'], pre_dis=0)
|
|
green_target_pose = self.get_grasp_pose_from_goal_point_and_direction(green_bottle, green_bottle_data, endpose_tag="right", actor_functional_point_id=0, target_point=right_target_pose, target_approach_direction=self.world_direction_dic['top_down'], pre_dis=0)
|
|
|
|
# Move both arms to the target positions
|
|
self.together_move_to_pose_with_screw(red_target_pose, green_target_pose)
|
|
|
|
# Note: The bottles are not put down at the end, as per the task description.
|
|
|
|
# Example usage:
|
|
# instance = gpt_dual_bottles_pick_easy()
|
|
# instance.play_once()
|