250 lines
17 KiB
Python
250 lines
17 KiB
Python
# ====================== PROMPT =============================
|
|
|
|
BASIC_INFO = '''
|
|
In this environment, distance 1 indicates 1 meter long. Pose is representated as 7 dimention, [x, y, z, qw, qx, qy, qz].
|
|
For a 7-dimensional Pose object, you can use Pose.p to get the [x, y, z] coordinates and Pose.q to get the [qw, qx, qy, qz] quaternion orientation.
|
|
All functions which has parameter actor_data, and all of actor_data should be in the actor_data_dic.
|
|
In the world coordinate system, the positive directions of the xyz coordinate axes are right, front, and upper respectively, so the direction vectors on the right, front,
|
|
and upper sides are [1,0,0], [0,1,0], [0,0,1] respectively. In the same way, we can get the unit vectors of the left side, back side and down side.
|
|
'''
|
|
|
|
CODE_TEMPLATE = '''
|
|
from .base_task import Base_task
|
|
from .$TASK_NAME$ import $TASK_NAME$
|
|
from .utils import *
|
|
import sapien
|
|
|
|
class gpt_$TASK_NAME$($TASK_NAME$):
|
|
def play_once(self):
|
|
pass
|
|
'''
|
|
|
|
|
|
FUNCTION_EXAMPLE = '''
|
|
You can retrieve the actor object by the actor's name:
|
|
```python
|
|
actor = self.actor_name_dic['actor_name']
|
|
```
|
|
You can retrieve the actor_data object by the actor_data's name:
|
|
```python
|
|
actor_data = self.actor_data_dic['actor_data_name']
|
|
```
|
|
|
|
Here are some APIs and examples of grasping objects:
|
|
If you want to get the gripper pose to grasp the actor, you typically execute the following code:
|
|
```python
|
|
grasp_pose = self.get_grasp_pose_to_grasp_object(endpose_tag = "left", self.actor, self.actor_data, pre_dis = 0.09) # endpose_tag can be "left" or "right"
|
|
```
|
|
|
|
If you want to pick up an actor, you can refer to the following sample code:
|
|
```python
|
|
pre_grasp_pose = self.get_grasp_pose_to_grasp_object(endpose_tag = "left", self.actor, self.actor_data, pre_dis = 0.09) # endpose_tag can be "left" or "right"
|
|
target_grasp_pose = self.get_grasp_pose_to_grasp_object(endpose_tag = "left", self.actor, self.actor_data, pre_dis = 0) # endpose_tag can be "left" or "right"
|
|
self.left_move_to_pose_with_screw(pre_grasp_pose) # left arm move to the pre grasp pose
|
|
self.left_move_to_pose_with_screw(target_grasp_pose) # left arm move to the grasp pose
|
|
self.close_left_gripper() # close left gripper to grasp the actor
|
|
self.left_move_to_pose_with_screw(pre_grasp_pose) # lift the actor up
|
|
```
|
|
The code for grasping with the right arm or both arms is similar to the above code.
|
|
|
|
For the grasping of a certain actor, the movement of the end-effector typically executes the following codes:
|
|
```python
|
|
actor_pose = self.get_actor_goal_pose(self.actor, self.actor_data)
|
|
|
|
if actor_pose[0] > 0: # if the actor in the right side, use right arm to grasp the actor
|
|
# grasp actor with right arm
|
|
else: # if the actor in the left side, use left arm to grasp the actor
|
|
# grasp actor with left arm
|
|
```
|
|
|
|
Here are some examples of gripper control:
|
|
```python
|
|
self.close_left_gripper(pos = 0.02) # Close half of the left gripper
|
|
self.close_left_gripper(pos = -0.01) # Tighten the left gripper.
|
|
self.open_left_gripper(pos = 0.02) # Open half of the left gripper
|
|
self.close_right_gripper(pos = 0.02) # Close half of the right gripper
|
|
self.close_right_gripper(pos = -0.01) # Tighten the right gripper.
|
|
self.open_right_gripper(pos = 0.02) # Open half of the right gripper
|
|
self.together_close_gripper(left_pos = 0.02,right_pose = 0.02) # Together close half of grippers
|
|
```
|
|
Note:
|
|
For grabbing some objects, you may need to close the clamping jaws tightly to grab them. You can adjust this through the 'pos' parameter, like 'pos = -0.01'.
|
|
By default 'pos' is 0, when close gripper.
|
|
|
|
Here are some APIs and examples of moving objects:
|
|
Note: The drop height of the actor depends on the distance of the actor that was lifted up the previous action.
|
|
To move an object to the target point, the 'get_grasp_pose_from_goal_point_and_direction()' is often called first to obtain the target's gripper posture.
|
|
|
|
If you want to move the point of actor which is grasped by the gripper action to the target point, you typically execute the following code:
|
|
```python
|
|
pre_grasp_pose = self.get_grasp_pose_from_goal_point_and_direction(self.actor, self.actor_data, endpose_tag = "left", actor_functional_point_id = 0, target_pose, target_approach_direction, pre_dis = 0.09)
|
|
target_grasp_pose = self.get_grasp_pose_from_goal_point_and_direction(self.actor, self.actor_data, endpose_tag = "left", actor_functional_point_id = 0, target_pose, target_approach_direction, pre_dis = 0)
|
|
self.left_move_to_pose_with_screw(pre_grasp_pose) # left arm move to the pre grasp pose
|
|
self.left_move_to_pose_with_screw(target_grasp_pose) # left arm move to the grasp pose
|
|
self.open_left_gripper() # open left gripper to place the target object
|
|
# You also can move right arm
|
|
```
|
|
Note:
|
|
1. The target_approach_direction is the approach direction which the actor's expected approach direction at the target point.
|
|
2. actor_functional_point_id is the index of the functional point of the actor, You can choose based on the given function points information.
|
|
3. For the parameter target_approach_direction, you can use self.world_direction_dic['left', 'front_left', 'front', 'fron_right', 'right', 'top_down'].
|
|
4. The target pose can be obtained by calling the 'get_actor_goal_pose()' function.
|
|
|
|
If you also have requirements for the target orientation of the object, you can specify the actor_target_orientation parameter through the direction vector to determine the final orientation of the object:
|
|
```python
|
|
# the actor target orientation is front, the direction vector is [0,1,0]
|
|
# The positive directions of the direction vector xyz axis are right, front, and up respectively.
|
|
pre_grasp_pose = self.get_grasp_pose_from_goal_point_and_direction(self.actor, self.actor_data, endpose_tag = "left", actor_functional_point_id = 0, target_pose, actor_target_orientation = [0,1,0], target_approach_direction, pre_dis = 0.09)
|
|
target_grasp_pose = self.get_grasp_pose_from_goal_point_and_direction(self.actor, self.actor_data, endpose_tag = "left", actor_functional_point_id = 0, target_pose, actor_target_orientation = [0,1,0], target_approach_direction, pre_dis = 0)
|
|
self.left_move_to_pose_with_screw(pre_grasp_pose) # left arm move to the pre grasp pose
|
|
self.left_move_to_pose_with_screw(target_grasp_pose) # left arm move to the grasp pose
|
|
self.open_left_gripper() # open left gripper to place the target object
|
|
```
|
|
|
|
If you need to align the functional axis of the grabbed object with the functional axis of the target object, you can use the following code:
|
|
```python
|
|
target_actor_functional_pose = self.get_actor_functional_pose(self.actor, self.actor_data, actor_functional_point_id = 0)
|
|
target_actor_point = target_actor_functional_pose[:3]
|
|
target_approach_direction = target_actor_functional_pose[3:]
|
|
pre_grasp_pose = self.get_grasp_pose_from_goal_point_and_direction(self.actor, self.actor_data, endpose_tag = "left", actor_functional_point_id = 0, target_point = target_actor_point, target_approach_direction = target_approach_direction, pre_dis = 0.09)
|
|
target_grasp_pose = self.get_grasp_pose_from_goal_point_and_direction(self.actor, self.actor_data, endpose_tag = "left", actor_functional_point_id = 0, target_point = target_actor_point, target_approach_direction = target_approach_direction, pre_dis = 0)
|
|
self.left_move_to_pose_with_screw(pre_grasp_pose) # left arm move to the pre grasp pose
|
|
self.left_move_to_pose_with_screw(target_grasp_pose) # left arm move to the grasp pose
|
|
self.open_left_gripper() # open left gripper to place the target object
|
|
```
|
|
Note:
|
|
1. The parameter actor in get_grasp_pose_from_goal_point_and_direction() should be grasp actor, not the target actor.
|
|
2. self.world_direction_dic is a dict of different approach directions.
|
|
3. This situation usually occurs when hanging objects or performing some delicate operations.
|
|
4. actor_functional_point_id is the index of the functional point of the actor, You can choose based on the given function points information.
|
|
|
|
Some tasks involve simultaneous operations of the left and right arms, which may require calling the collision avoidance function:
|
|
There is no need to avoid collision at the end of the task.
|
|
If both arms have moved at the same time before, and the next step needs to be to move the left arm first to place the target object, You can first obtain the pose of the right arm that can avoid subsequent collisions, and then move both arms at the same time:
|
|
```python
|
|
# Get left and right arm target pose
|
|
# Here, the direction in which the object contacts the target point is vertically top_down as an example.
|
|
# The actor target orientation is left, the direction vector is [-1,0,0].
|
|
left_pre_pose = self.get_grasp_pose_from_goal_point_and_direction(left_actor, left_actor_data, endpose_tag="left", actor_functional_point_id = 0, target_point=point1, target_approach_direction=self.world_direction_dic['top_down'],
|
|
actor_target_orientation=[-1, 0, 0], pre_dis=0.05)
|
|
left_target_pose = self.get_grasp_pose_from_goal_point_and_direction(left_actor, left_actor_data, endpose_tag="left", actor_functional_point_id = 0, target_point=point1, target_approach_direction=self.world_direction_dic['top_down'],
|
|
actor_target_orientation=[-1, 0, 0], pre_dis=0)
|
|
right_pre_pose = self.get_grasp_pose_from_goal_point_and_direction(right_actor, right_actor_data, endpose_tag="right", actor_functional_point_id = 0, target_point=point2, target_approach_direction=self.world_direction_dic['top_down'],
|
|
actor_target_orientation=[-1, 0, 0], pre_dis=0.05)
|
|
right_target_pose = self.get_grasp_pose_from_goal_point_and_direction(right_actor, right_actor_data, endpose_tag="right", actor_functional_point_id = 0, target_point=point2, target_approach_direction=self.world_direction_dic['top_down'],
|
|
actor_target_orientation=[-1, 0, 0], pre_dis=0)
|
|
# right arm avoid collision pose
|
|
right_avoid_collision_pose = self.get_avoid_collision_pose(avoid_collision_arm_tag = 'right')
|
|
# move left arm to the pre pose and right arm to the avoid collision pose
|
|
self.together_move_to_pose_with_screw(left_pre_pose, right_avoid_collision_pose)
|
|
# put down the actor on left gripper
|
|
self.left_move_to_pose_with_screw(left_target_pose)
|
|
self.open_left_gripper() # open left gripper to place the target object
|
|
# left arm avoid collision pose
|
|
left_avoid_collision_pose = self.get_avoid_collision_pose(avoid_collision_arm_tag = 'left')
|
|
# move right arm to the target pose and left arm to the avoid collision pose
|
|
self.together_move_to_pose_with_screw(left_avoid_collision_pose, right_pre_pose)
|
|
# put down the actor on right gripper
|
|
self.right_move_to_pose_with_screw(right_target_pose)
|
|
self.open_right_gripper() # open right gripper to place the target object
|
|
# avoid_collision_arm_tag: 'left' or 'right'
|
|
# direction: 'left', 'right', 'front', 'back', 'up', 'down'
|
|
```
|
|
Note:
|
|
1. If the move_arm_tag is 'left', the direction also not be 'right', and same for 'right'.
|
|
2. Collision avoidance may only be necessary if both arms have been moved.
|
|
'''
|
|
|
|
AVAILABLE_CONSTANTS = {
|
|
'self.world_direction_dic':'''{
|
|
'left': [0.5, 0.5, 0.5, 0.5],
|
|
'front_left': [0.65334811, 0.27043713, 0.65334811, 0.27043713],
|
|
'front' : [0.707, 0, 0.707, 0],
|
|
'front_right': [0.65334811, -0.27043713, 0.65334811, -0.27043713],
|
|
'right': [0.5, -0.5, 0.5, 0.5],
|
|
'top_down': [0, 0, 1, 0],
|
|
}
|
|
The world_direction_dic is a dict of different approach directions.
|
|
''',
|
|
}
|
|
AVAILABLE_ENV_FUNCTOIN = {
|
|
"open_left_gripper": "Open the left gripper to a specified position.",
|
|
"close_left_gripper": "Close the left gripper to a specified position.",
|
|
"open_right_gripper": "Open the right gripper to a specified position.",
|
|
"close_right_gripper": "Close the right gripper to a specified position.",
|
|
"together_open_gripper": "Open both left and right grippers to specified positions.",
|
|
"together_close_gripper": "Close both left and right grippers to specified positions.",
|
|
|
|
"left_move_to_pose_with_screw":
|
|
"def left_move_to_pose_with_screw(pose).\
|
|
Plan and execute a motion for the left arm using screw motion interpolation.\
|
|
No Return.\
|
|
Args:\
|
|
pose: list [x, y, z, qw, qx, qy, qz], the target pose of left end-effector",
|
|
"right_move_to_pose_with_screw":
|
|
"def right_move_to_pose_with_screw(pose).\
|
|
Plan and execute a motion for the right arm using screw motion interpolation.\
|
|
No Return.\
|
|
Args:\
|
|
pose: list [x, y, z, qw, qx, qy, qz], the target pose of right end-effector",
|
|
"together_move_to_pose_with_screw":
|
|
"def together_move_to_pose_with_screw(left_target_pose, right_target_pose).\
|
|
Plan and execute motions for both left and right arms using screw motion interpolation.\
|
|
No Return.\
|
|
Args:\
|
|
left_target_pose: list [x, y, z, qw, qx, qy, qz], the target pose of left end-effector\
|
|
right_target_pose: list [x, y, z, qw, qx, qy, qz], the target pose of right end-effector",
|
|
|
|
"get_actor_functional_pose":
|
|
"def get_actor_functional_pose(actor, actor_data),\
|
|
Get the functional pose of the actor in the world coordinate system.\
|
|
Returns: pose: list [x, y, z, qw, qx, qy, qz].\
|
|
Args:\
|
|
actor: Object(self.actor), the object of actor in render.\
|
|
actor_data: dict(self.actor_data), the actor_data match with actor.",
|
|
|
|
"get_grasp_pose_to_grasp_object":
|
|
"def get_grasp_pose_to_grasp_object(self, endpose_tag: str, actor, actor_data = DEFAULT_ACTOR_DATA, pre_dis = 0),\
|
|
This function is used to grasp actor from the labeled contact points of the actor, and return the most suitable pose of the end-effector.\
|
|
Returns: pose: list [x, y, z, qw, qx, qy, qz].\
|
|
Args:\
|
|
endpose_tag: str, the endpose tag of the actor, can be 'left' or 'right'.\
|
|
actor: Object(self.actor), the object of actor in render.\
|
|
actor_data: dict(self.actor_data), the actor_data match with actor.\
|
|
pre_dis: float, the distance between grasp pose and target actor pose.",
|
|
|
|
"get_grasp_pose_from_goal_point_and_direction":
|
|
"def get_grasp_pose_from_goal_point_and_direction(self, actor, actor_data, endpose_tag: str, actor_functional_point_id, target_point,\
|
|
target_approach_direction, actor_target_orientation = None, pre_dis):\
|
|
This function is used to move the actor's point of action to the target point when the direction of the end-effector is given, return the pose of the end-effector.\
|
|
The actor refers to an object being grasped by robotic grippers. actor_target_orientation is the orientation of the actor after grasping.\
|
|
Returns: pose: list [x, y, z, qw, qx, qy, qz].\
|
|
Args: \
|
|
actor: Object(self.actor), the object of actor in render.\
|
|
actor_data: dict(self.actor_data), the actor_data match with actor.\
|
|
endpose_tag: str, the endpose tag of the actor, can be 'left' or 'right'.\
|
|
actor_functional_point_id: int, the index of the functional point of the actor.\
|
|
target_point: list [x, y, z], the target point pose which the actor's target_pose expected to move to.\
|
|
target_approach_direction: list [qw, qx, qy, qz], the approach direction which the actor's expected approach direction at the target point. \
|
|
The target approach direction can use self.world_direction_dic['left', 'front_left', 'front', 'fron_right', 'right', 'top_down'].\
|
|
actor_target_orientation: list [x, y, z], the orientation of the actor after grasping.\
|
|
The positive directions of the xyz axis are right, front, and up respectively. You can give a direction vector to specify the target direction of the object.\
|
|
like [0, 0, 1] means the actor' orientation is up and [0, 1, 0] means the actor's orientation is front.\
|
|
pre_dis: float, the distance on approach direction between actor's point of action and target point.",
|
|
|
|
"get_avoid_collision_pose":
|
|
"def get_avoid_collision_pose(self, avoid_collision_arm_tag: str),\
|
|
This function can obtain the safe position of the specified robot arm to avoid collision when both arms need to move at the same time.\
|
|
Returns: pose: list [x, y, z, qw, qx, qy, qz].\
|
|
Args:\
|
|
avoid_collision_arm_tag: str, 'left' or 'right'.",
|
|
|
|
"get_actor_goal_pose":
|
|
"def get_actor_goal_pose(self, actor, actor_data, id),\
|
|
This function is used to get the target pose point of an actor in world axis.\
|
|
Returns: pose: list [x, y, z].\
|
|
Args:\
|
|
actor: Object(self.actor), the object of actor in render.\
|
|
actor_data: dict(self.actor_data), the actor_data match with actor.\
|
|
id: int, the id of the actor, if the actor has multiple target points. And default is 0.",
|
|
} |