hofee/gen_data_agent
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

finshi press action

Browse Source
This commit is contained in:
hofee
2025-09-24 11:11:24 +08:00
parent 8b89b2e35b
commit 330d903047
16 changed files with 296 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
data_gen_dependencies/action/__init__.py
View File

@@ -4,7 +4,9 @@ from .insert import InsertStage, HitStage
from .slide import SlideStage
from .pour import PourStage
from .pull import PullStage
from .push import PushStage, ClickStage
from .push import PushStage
from .pull_revolute import PullRevoluteStage
from .press import PressPrismaticStage
ACTION_STAGE = {
"grasp": GraspStage,
@@ -19,9 +21,11 @@ ACTION_STAGE = {
"hit": NotImplemented,
"pour": PourStage,
"push": PushStage,
'click': ClickStage,
'touch': ClickStage,
"pull": PullStage
'click': PressPrismaticStage,
'touch': PressPrismaticStage,
'press_prismatic': PressPrismaticStage,
"pull": PullStage,
"pull_revolute": PullRevoluteStage,
}
def build_stage(action):

1
data_gen_dependencies/action/base.py
View File

@@ -27,7 +27,6 @@ def simple_check_completion(goal, objects, last_statement=None, pos_threshold=0.
def solve_target_gripper_pose(stage, objects):
active_obj_ID, passive_obj_ID, target_pose_canonical, gripper_action, transform_world, motion_type = stage
anchor_pose = objects[passive_obj_ID].obj_pose

3
data_gen_dependencies/action/grasp.py
View File

@@ -4,7 +4,7 @@ from data_gen_dependencies.action.base import StageTemplate, simple_check_comple
class PickStage(StageTemplate):
def __init__(self, active_obj_id, passive_obj_id, active_element, passive_element, grasp_pose, extra_params=None, **kwargs):
def __init__(self, active_obj_id, passive_obj_id, active_element, passive_element, grasp_pose, extra_params=None, active_obj=None, passive_obj=None, **kwargs):
super().__init__(active_obj_id, passive_obj_id, active_element, passive_element)
self.grasp_pose_canonical = grasp_pose
self.use_pre_grasp = False
@@ -104,6 +104,7 @@ class GraspStage(PickStage):
self.sub_stages.append([pre_grasp_pose, None, np.eye(4), motion_type])
# sub-stage-1 moveTo grasp pose
self.sub_stages.append([grasp_pose, 'close', np.eye(4), motion_type])

2
data_gen_dependencies/action/insert.py
View File

@@ -4,7 +4,7 @@ from data_gen_dependencies.action.base import StageTemplate, simple_check_comple
class InsertStage(StageTemplate):
def __init__(self, active_obj_id, passive_obj_id, active_element=None, passive_element=None, target_pose=None, vector_direction=None, extra_params={}, **kwargs):
def __init__(self, active_obj_id, passive_obj_id, active_element=None, passive_element=None, target_pose=None, vector_direction=None, extra_params={}, active_obj=None, passive_obj=None, **kwargs):
super().__init__(active_obj_id, passive_obj_id, active_element, passive_element)

2
data_gen_dependencies/action/place.py
View File

@@ -4,7 +4,7 @@ from data_gen_dependencies.action.base import StageTemplate, simple_check_comple
class PlaceStage(StageTemplate):
def __init__(self, active_obj_id, passive_obj_id, active_element=None, passive_element=None, target_pose=None, extra_params={}, **kwargs):
def __init__(self, active_obj_id, passive_obj_id, active_element=None, passive_element=None, target_pose=None, extra_params={}, active_obj=None, passive_obj=None, **kwargs):
super().__init__(active_obj_id, passive_obj_id, active_element, passive_element)
self.pre_transform_up =0.12
self.place_transform_up = np.array([0, 0, 0.01])

2
data_gen_dependencies/action/pour.py
View File

@@ -9,7 +9,7 @@ import numpy as np
class PourStage(StageTemplate):
def __init__(self, active_obj_id, passive_obj_id, active_element=None, passive_element=None, target_pose=None, current_pose=None, obj2part=None, **kwargs):
def __init__(self, active_obj_id, passive_obj_id, active_element=None, passive_element=None, target_pose=None, current_pose=None, obj2part=None, active_obj=None, passive_obj=None, **kwargs):
super().__init__(active_obj_id, passive_obj_id, active_element, passive_element)
self.pre_pour_height = 0.25

34
data_gen_dependencies/action/press.py Normal file
View File

@@ -0,0 +1,34 @@
import numpy as np
from data_gen_dependencies.action.base import StageTemplate
class PressPrismaticStage(StageTemplate):
def __init__(self, active_obj_id, passive_obj_id, active_element=None, passive_element=None, target_pose=None, vector_direction=None,active_obj=None, passive_obj=None, **kwargs):
super().__init__(active_obj_id, passive_obj_id, active_element, passive_element)
self.pre_distance = -0.03
self.passive_obj = passive_obj
self.active_obj = active_obj
self.joint_position_threshold = passive_element.get('joint_position_threshold', 0.7)
self.joint_position_threshold = self.joint_position_threshold if self.joint_position_threshold is not None else 0.7
self.correspond_joint_id = passive_element.get('correspond_joint_id', None)
correspond_joint_info = passive_obj.joints_info[self.correspond_joint_id]
self.joint_lower_limit = correspond_joint_info["lower_bound"]
self.joint_upper_limit = correspond_joint_info["upper_bound"]
self.move_distance = -0.005 + abs(self.joint_lower_limit - self.joint_upper_limit) * self.joint_position_threshold
self.joint_axis = correspond_joint_info["joint_axis"]
self.joint_type = correspond_joint_info["joint_type"]
assert self.joint_type == 'prismatic', "joint_type must be prismatic for press action"
correspond_joint_info = passive_obj.joints_info[self.correspond_joint_id]
self.generate_substage(target_pose, vector_direction)
def generate_substage(self, target_pose, vector_direction):
vector_direction = vector_direction / np.linalg.norm(vector_direction)
pre_pose = target_pose.copy()
pre_pose[:3,3] += vector_direction * self.pre_distance
self.sub_stages.append([pre_pose, 'close', np.eye(4), 'AvoidObs'])
move_pose = target_pose.copy()
move_pose[:3,3] += vector_direction * self.move_distance
self.sub_stages.append([move_pose, None, np.eye(4), 'Simple'])

4
data_gen_dependencies/action/pull.py
View File

@@ -4,7 +4,7 @@ from data_gen_dependencies.action.base import StageTemplate
class PullStage(StageTemplate):
def __init__(self, active_obj_id, passive_obj_id, active_element=None, passive_element=None, target_pose=np.eye(4), extra_params=None, **kwargs):
def __init__(self, active_obj_id, passive_obj_id, active_element=None, passive_element=None, target_pose=np.eye(4), extra_params=None, active_obj=None, passive_obj=None, **kwargs):
super().__init__(active_obj_id, passive_obj_id, active_element, passive_element)
self.pull_distance = passive_element['distance']
assert self.pull_distance > 0
@@ -32,8 +32,6 @@ class PullStage(StageTemplate):
free_delta_pose[2,3] = -0.03
self.sub_stages.append([free_delta_pose, None, np.eye(4), 'local_gripper'])
def check_completion(self, objects):
if self.__len__()==0:

110
data_gen_dependencies/action/pull_revolute.py Normal file
View File

@@ -0,0 +1,110 @@
import numpy as np
from data_gen_dependencies.action.base import StageTemplate
class PullRevoluteStage(StageTemplate):
DELTA_DISTANCE = 0.005 # meter
def __init__(self, active_obj_id, passive_obj_id, active_element=None, passive_element=None, target_pose=np.eye(4), extra_params=None, active_obj=None, passive_obj=None, **kwargs):
super().__init__(active_obj_id, passive_obj_id, active_element, passive_element)
#======= DEBUG START =======
print(f'debug: PullRevoluteStage init')
import ipdb;ipdb.set_trace()
#======= DEBUG END =======
self.passive_obj = passive_obj
self.active_obj = active_obj
self.joint_position_threshold = passive_element.get('joint_position_threshold', 0.7)
self.correspond_joint_id = passive_element.get('correspond_joint_id', None)
correspond_joint_info = passive_obj.joints_info[self.correspond_joint_id]
self.joint_lower_limit = correspond_joint_info["lower_bound"]
self.joint_upper_limit = correspond_joint_info["upper_bound"]
self.joint_axis = correspond_joint_info["joint_axis"]
self.joint_type = correspond_joint_info["joint_type"]
assert self.joint_type == 'revolute', "joint_type must be revolute for pull_revolute action"
self.revolute_radius = passive_element.get('revolute_radius', None)
assert self.revolute_radius is not None, "revolute_radius is required for pull_revolute action"
if self.joint_position_threshold is None:
self.joint_position_threshold = 0.7
assert self.joint_position_threshold >= 0 and self.joint_position_threshold <= 1
self.joint_direction = passive_element.get('joint_direction', 1)
assert self.joint_direction in [-1, 1]
self.joint_velocity_threshold = passive_element.get('joint_velocity_threshold', 999)
vector_direction = passive_element['direction']
self.extra_params = {} if extra_params is None else extra_params
self.generate_substage(target_pose, vector_direction)
def generate_substage(self, target_pose, vector_direction):
vector_direction = vector_direction / np.linalg.norm(vector_direction)
#trajectory = self.test_generate_trajectory(vector_direction)
#trajectory = self.test_generate_trajectory(vector_direction)
total_angle = self.joint_position_threshold * (self.joint_upper_limit - self.joint_lower_limit)
arc_length = abs(self.revolute_radius * total_angle)
step = int(arc_length / self.DELTA_DISTANCE)
from pyboot.utils.log import Log
Log.debug(f'PullRevoluteStage: total_angle={total_angle}, arc_length={arc_length}, step={step}')
for i in range(10):
free_delta_pose = np.eye(4)
free_delta_pose[2,3] = -self.DELTA_DISTANCE
self.sub_stages.append([target_pose, None, np.eye(4), 'AvoidObs'])
self.sub_stages.append([free_delta_pose, None, np.eye(4), 'local_gripper'])
self.sub_stages.append([None, "open", np.eye(4), 'Simple'])
free_delta_pose = np.eye(4)
free_delta_pose[2,3] = -self.DELTA_DISTANCE
self.sub_stages.append([free_delta_pose, None, np.eye(4), 'local_gripper'])
def generate_trajectory(self, init_vector_direction):
trajectory = []
#======= DEBUG START =======
print(f'debug: ')
import ipdb;ipdb.set_trace()
#======= DEBUG END =======
total_angle = self.joint_position_threshold * (self.joint_upper_limit - self.joint_lower_limit)
total_angle *= self.joint_direction
arc_length = abs(self.revolute_radius * total_angle)
num_steps = int(np.ceil(arc_length / self.DELTA_DISTANCE))
if num_steps < 1:
num_steps = 1
delta_theta = total_angle / num_steps
rotation_axis = np.array(self.joint_axis)
current_pose = start_pose.copy()
trajectory.append(current_pose)
for i in range(1, num_steps + 1):
angle = i * delta_theta
R = self.axis_angle_to_matrix(rotation_axis, angle)
move_pose = start_pose @ R
trajectory.append(move_pose)
return trajectory
def axis_angle_to_matrix(self, axis, angle):
axis = axis / np.linalg.norm(axis)
K = np.array([[0, -axis[2], axis[1]],
[axis[2], 0, -axis[0]],
[-axis[1], axis[0], 0]])
R = np.eye(3) + np.sin(angle) * K + (1 - np.cos(angle)) * (K @ K)
T = np.eye(4)
T[:3, :3] = R
return T
def check_completion(self, objects):
if self.__len__() == 0:
return True
succ = False
if self.step_id >= 0:
succ = True
if succ:
self.step_id += 1
return succ

20
data_gen_dependencies/action/push.py
View File

@@ -4,7 +4,7 @@ from data_gen_dependencies.action.base import StageTemplate, pose_difference
class PushStage(StageTemplate):
def __init__(self, active_obj_id, passive_obj_id, active_element=None, passive_element=None, target_pose=np.eye(4), vector_direction=None, extra_params=None, **kwargs):
def __init__(self, active_obj_id, passive_obj_id, active_element=None, passive_element=None, target_pose=np.eye(4), vector_direction=None, extra_params=None, active_obj=None, passive_obj=None, **kwargs):
super().__init__(active_obj_id, passive_obj_id, active_element, passive_element)
extra_params = {} if extra_params is None else extra_params
@@ -73,23 +73,5 @@ class PushStage(StageTemplate):
return succ
class ClickStage(StageTemplate):
def __init__(self, active_obj_id, passive_obj_id, active_element=None, passive_element=None, target_pose=None, vector_direction=None, **kwargs):
super().__init__(active_obj_id, passive_obj_id, active_element, passive_element)
self.pre_distance = -0.03
self.move_distance = 0.00
self.generate_substage(target_pose, vector_direction)
def generate_substage(self, target_pose, vector_direction):
vector_direction = vector_direction / np.linalg.norm(vector_direction)
# moveTo pre-place position
pre_pose = target_pose.copy()
pre_pose[:3,3] += vector_direction * self.pre_distance
self.sub_stages.append([pre_pose, 'close', np.eye(4), 'AvoidObs'])
# insert
move_pose = target_pose.copy()
move_pose[:3,3] += vector_direction * self.move_distance
self.sub_stages.append([move_pose, None, np.eye(4), 'Simple'])

2
data_gen_dependencies/action/slide.py
View File

@@ -4,7 +4,7 @@ from data_gen_dependencies.action.base import StageTemplate, pose_difference
class SlideStage(StageTemplate):
def __init__(self, active_obj_id, passive_obj_id, active_element=None, passive_element=None, target_pose=np.eye(4), **kwargs):
def __init__(self, active_obj_id, passive_obj_id, active_element=None, passive_element=None, target_pose=np.eye(4), active_obj=None, passive_obj=None, **kwargs):
super().__init__(active_obj_id, passive_obj_id, active_element, passive_element)
self.pre_distance = 0.0

52
data_gen_dependencies/action/twist.py Normal file
View File

@@ -0,0 +1,52 @@
import numpy as np
from data_gen_dependencies.action.base import StageTemplate
class TwistStage(StageTemplate):
DELTA_DISTANCE = 0.01 # meter
def __init__(self, active_obj_id, passive_obj_id, active_element=None, passive_element=None, target_pose=np.eye(4), extra_params=None, active_obj=None, passive_obj=None, **kwargs):
super().__init__(active_obj_id, passive_obj_id, active_element, passive_element)
self.passive_obj = passive_obj
self.active_obj = active_obj
self.joint_position_threshold = passive_element.get('joint_position_threshold', 0.7)
self.correspond_joint_id = passive_element.get('correspond_joint_id', None)
correspond_joint_info = passive_obj.joints_info[self.correspond_joint_id]
self.joint_lower_limit = correspond_joint_info["lower_bound"]
self.joint_upper_limit = correspond_joint_info["upper_bound"]
self.joint_axis = correspond_joint_info["joint_axis"]
self.joint_type = correspond_joint_info["joint_type"]
assert self.joint_type == 'revolute', "joint_type must be revolute for pull_revolute action"
self.revolute_radius = passive_element.get('revolute_radius', None)
assert self.revolute_radius is not None, "revolute_radius is required for pull_revolute action"
if self.joint_position_threshold is None:
self.joint_position_threshold = 0.7
assert self.joint_position_threshold >= 0 and self.joint_position_threshold <= 1
self.joint_direction = passive_element.get('joint_direction', 1)
assert self.joint_direction in [-1, 1]
self.joint_velocity_threshold = passive_element.get('joint_velocity_threshold', 999)
vector_direction = passive_element['direction']
self.extra_params = {} if extra_params is None else extra_params
self.generate_substage(target_pose, vector_direction)
def generate_substage(self, target_pose, vector_direction):
vector_direction = vector_direction / np.linalg.norm(vector_direction)
free_delta_pose = np.eye(4)
free_delta_pose[2,3] = -0.03
self.sub_stages.append([free_delta_pose, None, np.eye(4), 'local_gripper'])
def check_completion(self, objects):
if self.__len__() == 0:
return True
succ = False
if self.step_id >= 0:
succ = True
if succ:
self.step_id += 1
return succ