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

add articulation & robotiq_2f85 urdf

Browse Source
This commit is contained in:
hofee
2025-10-21 19:57:42 +08:00
parent 65a39a4994
commit 6f43878922
10 changed files with 572 additions and 8413 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
data_gen_dependencies/omniagent.py
View File

@@ -205,6 +205,23 @@ class Agent(BaseAgent):
Log.error('-- Grasp file empty: %s' % grasp_file)
return False
return True
def visualize_gripper_pose(self, gripper_pose):
canonical_gripper_pts = np.zeros((60, 3))
for i in range(10):
canonical_gripper_pts[i] = np.array([0, 0.01, 0.002*i])
canonical_gripper_pts[i+10] = np.array([0, 0.01, 0.002*i + 0.001])
canonical_gripper_pts[i+20] = np.array([0, -0.01, 0.002*i])
canonical_gripper_pts[i+30] = np.array([0, -0.01, 0.002*i + 0.001])
canonical_gripper_pts[i+40] = np.array([0, -0.01 + 0.002 * i, 0])
for i in range(5):
canonical_gripper_pts[i+50] = np.array([0, 0, -0.002*i])
canonical_gripper_pts[i+55] = np.array([0, 0, -0.002*i + 0.001])
canonical_gripper_pts -= np.array([0, 0, 0.002*10 + 0.001])
canonical_gripper_pts = canonical_gripper_pts.reshape(-1, 3)
#simport ipdb; ipdb.set_trace()
gripper_pts = (gripper_pose[:3, :3] @ canonical_gripper_pts.T).T + gripper_pose[:3, 3]
return gripper_pts
def run(self, task_list, camera_list, use_recording, workspaces, fps=10, render_semantic=False):
for index, task_file in enumerate(task_list):
@@ -229,7 +246,6 @@ class Agent(BaseAgent):
#print('Reset pose:', self.robot.reset_pose)
task_info = json.load(open(task_file, 'rb'))
import ipdb; ipdb.set_trace()
stages, objects = load_task_solution(task_info)
objects = self.update_objects(objects)
split_stages = split_grasp_stages(stages)
@@ -287,6 +303,11 @@ class Agent(BaseAgent):
target_pose=target_gripper_pose)
if target_gripper_pose is not None:
self.robot.move_pose(target_gripper_pose, motion_type, arm=arm, block=True)
# vis_gripper_pose = self.visualize_gripper_pose(target_gripper_pose)
# import ipdb;ipdb.set_trace()
# np.savetxt('debug_gripper.txt', vis_gripper_pose)
set_gripper_open = gripper_action == 'open'
set_gripper_close = gripper_action == 'close'
@@ -432,6 +453,7 @@ class Agent(BaseAgent):
break
Log.success('Finished executing task.')
time.sleep(0.5)
#import ipdb; ipdb.set_trace()
if self.attached_obj_id is None:
self.robot.client.DetachObj()
self.robot.client.stop_recording()