finshi press action
This commit is contained in:
@@ -3,6 +3,7 @@ import copy
|
||||
import json
|
||||
import os
|
||||
from sqlite3 import dbapi2
|
||||
from this import d
|
||||
import time
|
||||
|
||||
from pyboot.utils.log import Log
|
||||
@@ -247,25 +248,30 @@ def load_task_solution(task_info):
|
||||
obj_dir = obj_info["data_info_dir"]
|
||||
obj = OmniObject.from_obj_dir(obj_dir, obj_info=obj_info)
|
||||
objects[obj_id] = obj
|
||||
|
||||
if hasattr(obj, 'part_ids'):
|
||||
for part_id in obj.part_ids:
|
||||
if obj.is_articulated:
|
||||
for part_id in obj.parts_info:
|
||||
id = obj_id + '/%s' % part_id
|
||||
objects[id] = copy.deepcopy(obj)
|
||||
objects[id].name = id
|
||||
#====== DEBUG START ======
|
||||
print(f'debug: ')
|
||||
import ipdb;ipdb.set_trace()
|
||||
#====== DEBUG END ======
|
||||
if hasattr(obj, 'part_joint_limits') and obj.part_joint_limits is not None:
|
||||
objects[id].part_joint_limit = obj.part_joint_limits[part_id]
|
||||
if len(obj.part_ids):
|
||||
objects[id].size = obj.parts_info[part_id]['size']
|
||||
joint_name = obj.parts_info[part_id]['correspond_joint_id']
|
||||
objects[id].joint_name = joint_name
|
||||
if objects[id].joint_name is not None and joint_name in obj.joints_info:
|
||||
objects[id].joint_type = obj.joints_info[joint_name]['joint_type']
|
||||
objects[id].joint_axis = obj.joints_info[joint_name]['joint_axis']
|
||||
objects[id].lower_bound = obj.joints_info[joint_name]['lower_bound']
|
||||
objects[id].upper_bound = obj.joints_info[joint_name]['upper_bound']
|
||||
|
||||
if len(obj.parts_info):
|
||||
del objects[obj_id]
|
||||
|
||||
Log.success('Finished Loading task solution')
|
||||
|
||||
return stages, objects
|
||||
|
||||
|
||||
def parse_stage(stage, objects):
|
||||
|
||||
action = stage['action']
|
||||
if action in ['reset']:
|
||||
return action, 'gripper', 'gripper', None, None, None, None
|
||||
@@ -280,24 +286,36 @@ def parse_stage(stage, objects):
|
||||
active_obj = objects[active_obj_id]
|
||||
passive_obj = objects[passive_obj_id]
|
||||
|
||||
single_obj = action in ['pull', 'rotate', 'slide', 'shave', 'brush', 'wipe']
|
||||
single_obj = action in ['pull', 'rotate', 'slide', 'shave', 'brush', 'wipe', 'pull_revolute']
|
||||
|
||||
def _load_element(obj, type):
|
||||
|
||||
if action in ['pick', 'hook']:
|
||||
action_mapped = 'grasp'
|
||||
elif action in ['pull_revolute']:
|
||||
action_mapped = 'pull'
|
||||
elif action in ["press_prismatic"]:
|
||||
action_mapped = 'press'
|
||||
else:
|
||||
action_mapped = action
|
||||
if action_mapped == 'grasp' and type == 'active':
|
||||
if (action_mapped == 'grasp' or action_mapped == 'pull') and type == 'active':
|
||||
return None, None
|
||||
elif obj.name == 'gripper':
|
||||
element = obj.elements[type][action_mapped]
|
||||
return element, 'default'
|
||||
primitive = stage[type]['primitive'] if stage[type]['primitive'] is not None else 'default'
|
||||
if primitive != 'default' or (action_mapped == 'grasp' and type == 'passive'):
|
||||
if action_mapped not in obj.elements[type]:
|
||||
if action_mapped not in obj.elements[type] and action not in obj.elements[type]:
|
||||
#======= DEBUG START =======
|
||||
print(f'debug: ')
|
||||
import ipdb;ipdb.set_trace()
|
||||
#======= DEBUG END =======
|
||||
print('No %s element for %s' % (action_mapped, obj.name))
|
||||
return None, None
|
||||
element = obj.elements[type][action_mapped][primitive]
|
||||
elif action_mapped in obj.elements[type]:
|
||||
element = obj.elements[type][action_mapped][primitive]
|
||||
elif action in obj.elements[type]:
|
||||
element = obj.elements[type][action][primitive]
|
||||
else:
|
||||
element = []
|
||||
for primitive in obj.elements[type][action_mapped]:
|
||||
@@ -324,11 +342,10 @@ def select_obj(objects, stages, robot):
|
||||
|
||||
''' 初筛抓取pose,得到 grasp_poses_canonical, grasp_poses '''
|
||||
grasp_stage_id = None
|
||||
|
||||
if stages[0]['action'] in ['push', 'reset', 'click']:
|
||||
if stages[0]['action'] in ['push', 'reset', 'press_prismatic']:
|
||||
gripper2obj = current_gripper_pose
|
||||
|
||||
elif stages[0]['action'] in ['pick', 'grasp', 'hook']:
|
||||
elif stages[0]['action'] in ['pick', 'grasp', 'hook', "pull_revolute"]:
|
||||
action = stages[0]['action']
|
||||
|
||||
''' 筛掉无IK解的grasp pose '''
|
||||
@@ -386,6 +403,7 @@ def select_obj(objects, stages, robot):
|
||||
direction_canonical = (np.linalg.inv(obj_pose) @ np.array([*direction, 0]))[:3]
|
||||
active_elements = [{'xyz': xyz_canonical, 'direction': direction_canonical}]
|
||||
|
||||
|
||||
# import ipdb; ipdb.set_trace()
|
||||
t0 = time.time()
|
||||
element_ik_score = []
|
||||
@@ -504,7 +522,6 @@ def split_grasp_stages(stages):
|
||||
|
||||
def generate_action_stages(objects, all_stages, robot):
|
||||
split_stages = split_grasp_stages(all_stages)
|
||||
|
||||
action_stages = []
|
||||
for stages in split_stages:
|
||||
gripper2obj = select_obj(objects, stages, robot)
|
||||
@@ -512,30 +529,37 @@ def generate_action_stages(objects, all_stages, robot):
|
||||
print('No gripper2obj pose can pass IK')
|
||||
gripper2obj = select_obj(objects, stages, robot)
|
||||
return []
|
||||
stage_idx = 0
|
||||
for stage in stages:
|
||||
stage_idx += 1
|
||||
extra_params = stage.get('extra_params', {})
|
||||
arm = extra_params.get('arm', 'right')
|
||||
current_gripper_pose = robot.get_ee_pose(id=arm)
|
||||
action, active_obj_id, passive_obj_id, active_elements, passive_elements, active_primitive, passive_primitive = parse_stage(
|
||||
stage, objects)
|
||||
active_obj = objects[active_obj_id]
|
||||
passive_obj = objects[passive_obj_id]
|
||||
|
||||
single_obj = active_obj_id == passive_obj_id
|
||||
Log.info(f'Generating action stage [{stage_idx}/{len(stages)}]: Action({action}), Active Object({active_obj_id}), Passive Object({passive_obj_id})')
|
||||
|
||||
substages = None
|
||||
#======= DEBUG START =======
|
||||
print(f'debug: before build_stage {action}')
|
||||
#import ipdb;ipdb.set_trace()
|
||||
#======= DEBUG END =======
|
||||
if action in ['reset']:
|
||||
substages = True
|
||||
elif action in ['pick', 'grasp', 'hook']:
|
||||
substages = build_stage(action)(active_obj_id, passive_obj_id, active_elements, passive_elements,
|
||||
gripper2obj, extra_params=stage.get('extra_params', None))
|
||||
elif action in ['slide', 'shave', 'brush', 'wipe', 'pull', 'rotate', 'pull', 'move',
|
||||
'reset']: # grasp + 物体自身运动
|
||||
gripper2obj, extra_params=stage.get('extra_params', None), active_obj=active_obj, passive_obj=passive_obj)
|
||||
elif action in ['slide', 'shave', 'brush', 'wipe', 'pull', 'rotate', 'move',
|
||||
'reset', "pull_revolute", "twist"]: # grasp + 物体自身运动
|
||||
passive_element = passive_elements[np.random.choice(len(passive_elements))]
|
||||
substages = build_stage(action)(
|
||||
active_obj_id=active_obj_id,
|
||||
passive_obj_id=passive_obj_id,
|
||||
passive_element=passive_element
|
||||
passive_element=passive_element,
|
||||
active_obj=active_obj,
|
||||
passive_obj=passive_obj,
|
||||
target_pose=gripper2obj
|
||||
)
|
||||
else:
|
||||
passive_element = passive_elements[np.random.choice(len(passive_elements))]
|
||||
@@ -575,7 +599,7 @@ def generate_action_stages(objects, all_stages, robot):
|
||||
ik_jacobian_score = ik_info["jacobian_score"][ik_success]
|
||||
|
||||
if len(target_gripper_poses_pass_ik) == 0:
|
||||
print(action, ': No target_obj_pose can pass isaac curobo IK')
|
||||
Log.warning(f'{action}: No target_obj_pose can pass isaac curobo IK')
|
||||
continue
|
||||
|
||||
target_joint_positions = []
|
||||
@@ -617,12 +641,14 @@ def generate_action_stages(objects, all_stages, robot):
|
||||
obj2part=obj2part,
|
||||
vector_direction=passive_element['direction'],
|
||||
passive_element=passive_element,
|
||||
extra_params=stage.get('extra_params', {})
|
||||
extra_params=stage.get('extra_params', {}),
|
||||
active_obj=active_obj,
|
||||
passive_obj=passive_obj
|
||||
)
|
||||
break
|
||||
|
||||
if substages is None:
|
||||
print(action, ': No target_obj_pose can pass IK')
|
||||
Log.warning(f'{action}:substage is None')
|
||||
return []
|
||||
action_stages.append((action, substages))
|
||||
|
||||
|
||||
Reference in New Issue
Block a user