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

solve dependencies problem

Browse Source
This commit is contained in:
hofee
2025-09-05 15:49:00 +08:00
parent 12a6b47969
commit 21fbd5a323
114 changed files with 11337 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
data_gen_dependencies/action/__init__.py Normal file
View File

@@ -0,0 +1,30 @@
from .grasp import GraspStage, PickStage, HookStage
from .place import PlaceStage
from .insert import InsertStage, HitStage
from .slide import SlideStage
from .pour import PourStage
from .pull import PullStage
from .push import PushStage, ClickStage
ACTION_STAGE = {
"grasp": GraspStage,
"pick": PickStage,
"hook": HookStage,
"place": PlaceStage,
"insert": InsertStage,
"slide": SlideStage,
"shave": NotImplemented,
"brush": NotImplemented,
"wipe": NotImplemented,
"hit": NotImplemented,
"pour": PourStage,
"push": PushStage,
'click': ClickStage,
'touch': ClickStage,
"pull": PullStage
}
def build_stage(action):
if action not in ACTION_STAGE:
raise NotImplementedError
return ACTION_STAGE[action]

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

@@ -0,0 +1,110 @@
import copy
from collections import deque
import numpy as np
from data_gen_dependencies.data_utils import pose_difference
def simple_check_completion(goal, objects, last_statement=None, pos_threshold=0.06, angle_threshold=70, is_grasped=False):
active_obj_id, passive_obj_id, target_pose_canonical, gripper_action, transform_world, motion_type = goal
if target_pose_canonical is None:
return True
if gripper_action=='open':
return True
current_pose_world = objects[active_obj_id].obj_pose
if len(target_pose_canonical.shape)==3:
target_pose_canonical = target_pose_canonical[-1]
transform_world = transform_world[-1]
target_pose_world = objects[passive_obj_id].obj_pose @ target_pose_canonical
if not is_grasped:
target_pose_world = np.dot(transform_world, target_pose_world)
pos_diff, angle_diff = pose_difference(current_pose_world, target_pose_world)
success = (pos_diff < pos_threshold) and (angle_diff < angle_threshold)
return success
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
if motion_type=='Trajectory':
assert len(target_pose_canonical.shape)==3, 'The target_pose should be a list of poses'
target_pose = anchor_pose[np.newaxis, ...] @ target_pose_canonical
target_pose = transform_world @ target_pose
else:
target_pose = anchor_pose @ target_pose_canonical
target_pose = transform_world @ target_pose
assert 'gripper' in objects, 'The gripper should be the first one in the object list'
current_gripper_pose = objects['gripper'].obj_pose
if active_obj_ID=='gripper':
target_gripper_pose = target_pose
else:
current_obj_pose = objects[active_obj_ID].obj_pose
gripper2obj = np.linalg.inv(current_obj_pose) @ current_gripper_pose
if len(target_pose.shape)==3:
gripper2obj = gripper2obj[np.newaxis, ...]
target_obj_pose = target_pose
target_gripper_pose = target_obj_pose @ gripper2obj
return target_gripper_pose
class StageTemplate:
def __init__(self, active_obj_id, passive_obj_id, active_element, passive_element):
self.active_obj_id = active_obj_id
self.passive_obj_id = passive_obj_id
self.active_element = active_element
self.passive_element = passive_element
self.last_statement = None
self.sub_stages = deque()
self.step_id = 0
self.extra_params = {}
def generate_substage(self):
raise NotImplementedError
def __len__(self) -> int:
return len(self.sub_stages) - self.step_id
def get_action(self, objects):
if self.__len__()==0:
return None
gripper_pose_canonical, gripper_action, transform_world, motion_type = self.sub_stages[self.step_id]
if motion_type == 'local_gripper':
delta_pose = gripper_pose_canonical
gripper_pose = objects['gripper'].obj_pose
target_gripper_pose = gripper_pose @ delta_pose
motion_type = 'Straight'
else:
if gripper_pose_canonical is None:
target_gripper_pose = None
else:
goal_datapack = [self.active_obj_id, self.passive_obj_id] + self.sub_stages[self.step_id]
target_gripper_pose = solve_target_gripper_pose(goal_datapack, objects)
last_statement = {'objects': copy.deepcopy(objects), 'target_gripper_pose': target_gripper_pose}
self.last_statement = last_statement
return target_gripper_pose, motion_type, gripper_action, self.extra_params.get('arm', 'right')
def check_completion(self, objects):
if self.__len__()==0:
return True
goal_datapack = [self.active_obj_id, self.passive_obj_id] + self.sub_stages[self.step_id]
succ = simple_check_completion(goal_datapack, objects)
if succ:
self.step_id += 1
return succ

136
data_gen_dependencies/action/grasp.py Normal file
View File

@@ -0,0 +1,136 @@
import numpy as np
from data_gen_dependencies.action.base import StageTemplate, simple_check_completion, pose_difference
class PickStage(StageTemplate):
def __init__(self, active_obj_id, passive_obj_id, active_element, passive_element, grasp_pose, extra_params=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
self.extra_params = {} if extra_params is None else extra_params
self.pick_up_step = 999
self.generate_substage(grasp_pose)
def generate_substage(self, grasp_pose):
pick_up_distance = self.extra_params.get('pick_up_distance', 0.12)
pick_up_type = self.extra_params.get('pick_up_type', 'Simple')
pick_up_direction = self.extra_params.get('pick_up_direction', 'z')
num_against = self.extra_params.get('against', 0)
if self.use_pre_grasp:
pre_grasp_distance = self.extra_params.get('pre_grasp_distance', 0.08)
# sub-stage-0 moveTo pregrasp pose
pre_pose = np.array([
[1., 0, 0, 0],
[0, 1., 0, 0],
[0, 0, 1., -pre_grasp_distance],
[0, 0, 0, 1]])
pre_grasp_pose = grasp_pose @ pre_pose
self.sub_stages.append([pre_grasp_pose, None, np.eye(4), 'AvoidObs'])
# sub-stage-1 moveTo grasp pose
self.sub_stages.append([grasp_pose, 'close', np.eye(4), 'Simple'])
self.pick_up_step = 2
elif num_against > 0:
for index in range(num_against):
self.sub_stages.append([grasp_pose, 'open', np.eye(4), 'AvoidObs'])
self.sub_stages.append([grasp_pose, 'close', np.eye(4), 'AvoidObs'])
self.pick_up_step = num_against + 1
gripper_action = None
motion_type = pick_up_type
transform_up = np.eye(4)
if pick_up_direction == "x":
transform_up[0,3] = pick_up_distance
transform_up[2,3] = 0.02
elif pick_up_direction == "y":
transform_up[1,3] = pick_up_distance
else:
transform_up[2,3] = pick_up_distance
self.sub_stages.append([grasp_pose, gripper_action, transform_up, motion_type])
else:
# sub-stage-0 moveTo grasp pose
self.sub_stages.append([grasp_pose, 'close', np.eye(4), 'AvoidObs'])
self.pick_up_step = 1
# pick-up
gripper_action = None
motion_type = pick_up_type
transform_up = np.eye(4)
if pick_up_direction == "x":
transform_up[0,3] = pick_up_distance
transform_up[2,3] = 0.02
elif pick_up_direction == "y":
transform_up[1,3] = pick_up_distance
else:
transform_up[2,3] = pick_up_distance
self.sub_stages.append([grasp_pose, gripper_action, transform_up, motion_type])
def check_completion(self, objects):
if self.__len__()==0:
return True
goal_datapack = [self.active_obj_id, self.passive_obj_id] + self.sub_stages[self.step_id]
succ = True
if self.step_id < self.pick_up_step:
succ = simple_check_completion(goal_datapack, objects, is_grasped=True)
if succ:
self.step_id += 1
return succ
class GraspStage(PickStage):
def generate_substage(self, grasp_pose):
motion_type = self.extra_params.get('move_type', 'AvoidObs')
if self.extra_params.get('use_pre_grasp', False):
pre_pose = np.array([
[1., 0, 0, 0],
[0, 1., 0, 0],
[0, 0, 1., -0.08],
[0, 0, 0, 1]])
pre_grasp_pose = grasp_pose @ pre_pose
gripper_action = None
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])
def check_completion(self, objects):
if self.__len__()==0:
return True
goal_datapack = [self.active_obj_id, self.passive_obj_id] + self.sub_stages[self.step_id]
pos_threshold = self.extra_params.get('position_threshold', 0.06)
succ = simple_check_completion(goal_datapack, objects, pos_threshold=pos_threshold, is_grasped=True)
if self.step_id >= 2:
gripper_pose = objects['gripper'].obj_pose
grasp_pose = objects[self.passive_obj_id].obj_pose @ self.grasp_pose_canonical
pos_diff, _ = pose_difference(gripper_pose, grasp_pose)
grasp_succ = pos_diff < pos_threshold
succ = succ and grasp_succ
if succ:
self.step_id += 1
return succ
class HookStage(PickStage):
def generate_substage(self, grasp_pose, *args, **kwargs):
self.sub_stages.append([grasp_pose, None, np.eye(4), 'AvoidObs'])

53
data_gen_dependencies/action/insert.py Normal file
View File

@@ -0,0 +1,53 @@
import numpy as np
from data_gen_dependencies.action.base import StageTemplate, simple_check_completion
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):
super().__init__(active_obj_id, passive_obj_id, active_element, passive_element)
self.move_distance = 0.01
self.generate_substage(target_pose, vector_direction, extra_params)
def generate_substage(self, target_pose, vector_direction, extra_params={}):
insert_pre_distance = extra_params.get('insert_pre_distance', -0.08)
insert_motion_type = extra_params.get('insert_motion_type', 'Simple')
vector_direction = vector_direction / np.linalg.norm(vector_direction)
# moveTo pre-place position
pre_pose = target_pose.copy()
pre_pose[:3,3] += vector_direction * insert_pre_distance
self.sub_stages.append([pre_pose, None, 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), insert_motion_type])
# open gripper
self.sub_stages.append([None, 'open', np.eye(4), None])
def check_completion(self, objects):
if self.__len__()==0:
return True
goal_datapack = [self.active_obj_id, self.passive_obj_id] + self.sub_stages[self.step_id]
succ = simple_check_completion(goal_datapack, objects)
if succ:
self.step_id += 1
return succ
class HitStage(InsertStage):
def __init__(self, active_obj_id, passive_obj_id, active_element=None, passive_element=None, target_pose=np.eye(4), vector_direction=None, **kwargs):
super(InsertStage, self).__init__(active_obj_id, passive_obj_id, active_element, passive_element)
self.pre_distance = -0.05
self.move_distance = 0.005
self.generate_substage(target_pose, vector_direction)

64
data_gen_dependencies/action/place.py Normal file
View File

@@ -0,0 +1,64 @@
import numpy as np
from data_gen_dependencies.action.base import StageTemplate, simple_check_completion
class PlaceStage(StageTemplate):
def __init__(self, active_obj_id, passive_obj_id, active_element=None, passive_element=None, target_pose=None, extra_params={}, **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])
self.extra_params = {} if extra_params is None else extra_params
self.use_pre_place = self.extra_params.get('use_pre_place', True)
self.generate_substage(target_pose)
def generate_substage(self, target_pose):
target_pose_canonical = target_pose
gripper_cmd = self.extra_params.get('gripper_state', 'open')
pre_place_direction = self.extra_params.get('pre_place_direction', 'z')
print(gripper_cmd)
num_against = self.extra_params.get('against', 0)
if self.use_pre_place:
# moveTo pre-place position
transform_up = np.eye(4)
print(transform_up)
if pre_place_direction == "x":
transform_up[0,3] = self.pre_transform_up
transform_up[2,3] = 0.02
elif pre_place_direction == "y":
transform_up[1,3] = self.pre_transform_up
else:
transform_up[2,3] = self.pre_transform_up
self.sub_stages.append([target_pose_canonical, None, transform_up, 'AvoidObs'])
# place
palce_transform_up = np.eye(4)
palce_transform_up[:3,3] = self.place_transform_up
if num_against > 0:
for index in range(num_against):
self.sub_stages.append([target_pose_canonical, None, transform_up, 'AvoidObs'])
self.pick_up_step = num_against + 1
self.sub_stages.append([target_pose_canonical, gripper_cmd, palce_transform_up, 'Simple'])
else:
palce_transform_up = np.eye(4)
palce_transform_up[:3,3] = self.place_transform_up
self.sub_stages.append([target_pose_canonical, None, palce_transform_up, 'AvoidObs'])
self.sub_stages.append([None, gripper_cmd, np.eye(4), 'Simple'])
def check_completion(self, objects):
if self.__len__()==0:
return True
goal_datapack = [self.active_obj_id, self.passive_obj_id] + self.sub_stages[self.step_id]
succ = True
if self.step_id==0:
succ = simple_check_completion(goal_datapack, objects)
if succ:
self.step_id += 1
return succ

97
data_gen_dependencies/action/pour.py Normal file
View File

@@ -0,0 +1,97 @@
from data_gen_dependencies.action.base import StageTemplate, solve_target_gripper_pose, simple_check_completion
from data_gen_dependencies.fix_rotation import interpolate_rotation_matrices
import copy
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):
super().__init__(active_obj_id, passive_obj_id, active_element, passive_element)
self.pre_pour_height = 0.25
self.pour_heigth = 0.08
self.generate_substage(target_pose, current_pose, obj2part)
def generate_substage(self, target_pose, current_pose, obj2part):
target_part_pose = target_pose @ np.linalg.inv(obj2part)
current_part_pose = current_pose @ np.linalg.inv(obj2part)
gripper_action = None
transform_pre_pour = np.eye(4)
transform_pre_pour[2,3] = self.pre_pour_height # 8cm above the target pose
# moveTo pre-pour position
pre_pour_part_pose = np.copy(target_part_pose)
pre_pour_part_pose[:3, :3] = current_part_pose[:3, :3]
# # pre_pour at 2/3 position from current to target
# pos_diff = target_part_pose[:3, 3] - current_part_pose[:3, 3]
# pre_pour_part_pose[:3, 3] = current_part_pose[:3, 3] + pos_diff * 1/2
# pre_pour_part_pose[:3, :3] = current_part_pose[:3, :3]
pre_pour_pose = pre_pour_part_pose @ obj2part
motion_type = 'AvoidObs'
self.sub_stages.append([pre_pour_pose, gripper_action, transform_pre_pour, motion_type])
motion_type = 'Trajectory'
rotations = interpolate_rotation_matrices(current_part_pose[:3,:3], target_part_pose[:3,:3], 200)
target_part_pose_list = np.tile(target_part_pose, (len(rotations), 1, 1))
target_part_pose_list[:, :3, :3] = rotations
target_pose_list = target_part_pose_list @ obj2part[np.newaxis, ...]
transform_pour = np.tile(np.eye(4), (len(rotations), 1, 1))
# transform_pour[:, 2, 3] = self.pre_pour_height
transform_pour[:, 2, 3] = np.linspace(self.pre_pour_height, self.pour_heigth, len(rotations))
# import ipdb; ipdb.set_trace()
self.sub_stages.append([target_pose_list, gripper_action, transform_pour, motion_type])
def get_action(self, objects):
if self.__len__()==0:
return None
gripper_pose_canonical, gripper_action, transform_world, motion_type = self.sub_stages[self.step_id]
if gripper_pose_canonical is None:
target_gripper_pose = None
else:
goal_datapack = [self.active_obj_id, self.passive_obj_id] + self.sub_stages[self.step_id]
target_gripper_pose = solve_target_gripper_pose(goal_datapack, objects)
# current_gripper_pose = objects['gripper'].obj_pose
# if self.step_id==0:
# # pre_pour at 5cm away from current to target
# diff_xy_direction = target_gripper_pose[:2, 3] - current_gripper_pose[:2, 3]
# pos_diff = np.linalg.norm(diff_xy_direction) * 0.10
# target_gripper_pose[:2, 3] = current_gripper_pose[:2, 3] + pos_diff
# elif self.step_id==1:
# target_xyz = target_gripper_pose[-1, :3, 3]
# current_xyz = current_gripper_pose[:3, 3]
# xyz_interp = np.linspace(current_xyz, target_xyz, len(target_xyz))
# target_gripper_pose[:, :3, 3] = xyz_interp
# import ipdb; ipdb.set_trace()
last_statement = {'objects': copy.deepcopy(objects), 'target_gripper_pose': target_gripper_pose}
self.last_statement = last_statement
return target_gripper_pose, motion_type, gripper_action , "right"
def check_completion(self, objects):
if self.__len__()==0:
return True
goal_datapack = [self.active_obj_id, self.passive_obj_id] + self.sub_stages[self.step_id]
succ = simple_check_completion(goal_datapack, objects)
if succ:
self.step_id += 1
return succ

59
data_gen_dependencies/action/pull.py Normal file
View File

@@ -0,0 +1,59 @@
import numpy as np
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):
super().__init__(active_obj_id, passive_obj_id, active_element, passive_element)
self.pull_distance = passive_element['distance']
assert self.pull_distance > 0
self.joint_position_threshold = passive_element.get('joint_position_threshold', 0.5)
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
if 'pull_distance' in self.extra_params:
self.pull_distance = self.extra_params['pull_distance']
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
move_pose = target_pose.copy()
move_pose[:3,3] += vector_direction * self.pull_distance
self.sub_stages.append([move_pose, None, np.eye(4), 'Straight'])
self.sub_stages.append([None, "open", np.eye(4), 'Simple'])
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
if self.step_id >= 0:
joint_position = objects[self.passive_obj_id].joint_position
joint_velocity = objects[self.passive_obj_id].joint_velocity
lower_bound = objects[self.passive_obj_id].part_joint_limit['lower_bound']
upper_bound = objects[self.passive_obj_id].part_joint_limit['upper_bound']
if self.joint_direction == 1:
succ = joint_position > lower_bound + (upper_bound - lower_bound) * self.joint_position_threshold
else:
succ = joint_position < lower_bound + (upper_bound - lower_bound) * self.joint_position_threshold
succ = succ and joint_velocity < self.joint_velocity_threshold
if succ:
self.step_id += 1
# else:
# import ipdb;ipdb.set_trace()
return succ

95
data_gen_dependencies/action/push.py Normal file
View File

@@ -0,0 +1,95 @@
import numpy as np
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):
super().__init__(active_obj_id, passive_obj_id, active_element, passive_element)
extra_params = {} if extra_params is None else extra_params
self.pre_distance = extra_params.get('pre_distance', -0.03)
self.move_distance = extra_params.get('move_distance', 0.14)
vector_direction = vector_direction / np.linalg.norm(vector_direction)
self.goal_transform = np.eye(4)
self.goal_transform[:3,3] = vector_direction * self.move_distance
if passive_element is not None:
self.joint_position_threshold = passive_element.get('joint_position_threshold', 0.5)
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)
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), 'Straight'])
def check_completion(self, objects):
if self.__len__()==0:
return True
# TODO 铰接joitn判定
if self.step_id==0:
succ = True
if self.step_id>=1:
if objects[self.passive_obj_id].is_articulated:
joint_position = objects[self.passive_obj_id].joint_position
joint_velocity = objects[self.passive_obj_id].joint_velocity
lower_bound = objects[self.passive_obj_id].part_joint_limit['lower_bound']
upper_bound = objects[self.passive_obj_id].part_joint_limit['upper_bound']
if self.joint_direction == 1:
succ = joint_position > lower_bound + (upper_bound - lower_bound) * self.joint_position_threshold
else:
succ = joint_position < lower_bound + (upper_bound - lower_bound) * self.joint_position_threshold
succ = succ and joint_velocity < self.joint_velocity_threshold
else:
sub_stage = self.sub_stages[self.step_id]
last_pose = self.last_statement['objects'][self.passive_obj_id].obj_pose
target_obj_pose = last_pose @ self.goal_transform
current_obj_pose = objects[self.passive_obj_id].obj_pose
pos_diff, _ = pose_difference(current_obj_pose, target_obj_pose)
succ = pos_diff < 0.04
if succ:
self.step_id += 1
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'])

47
data_gen_dependencies/action/slide.py Normal file
View File

@@ -0,0 +1,47 @@
import numpy as np
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):
super().__init__(active_obj_id, passive_obj_id, active_element, passive_element)
self.pre_distance = 0.0
self.move_distance = 0.08
vector_direction = passive_element['direction']
self.generate_substage(target_pose, vector_direction)
def generate_substage(self, target_pose, vector_direction):
vector_direction = vector_direction / np.linalg.norm(vector_direction)
# slide
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'])
# open gripper
self.sub_stages.append([None, 'open', np.eye(4), None])
def check_completion(self, objects):
if self.__len__()==0:
return True
sub_stage = self.sub_stages[self.step_id]
move_transform, gripper_action = sub_stage[0], sub_stage[1]
if gripper_action=='open':
self.step_id += 1
return True
last_pose = self.last_statement['objects'][self.passive_obj_id].obj_pose
target_obj_pose = last_pose @ move_transform
current_obj_pose = objects[self.passive_obj_id].obj_pose
pos_diff, _ = pose_difference(current_obj_pose, target_obj_pose)
succ = pos_diff < 0.04
print(pos_diff, succ)
if succ:
self.step_id += 1
return succ