From 02440ceeb547332d475f1b9797e66b15ddcbd06c Mon Sep 17 00:00:00 2001 From: hofee Date: Fri, 5 Sep 2025 19:18:37 +0800 Subject: [PATCH] finish data generate --- configs/generate_data_config.yaml | 3 +- data_gen_dependencies/action/place.py | 4 +- data_gen_dependencies/action/slide.py | 2 +- data_gen_dependencies/base_agent.py | 23 +- data_gen_dependencies/client.py | 395 +-- data_gen_dependencies/data_generate.py | 12 +- data_gen_dependencies/object.py | 9 +- data_gen_dependencies/omni_robot.py | 6 +- data_gen_dependencies/omniagent.py | 430 ++- .../robot_urdf/A2D_120s.urdf | 2391 +++++++++++++++++ .../robot_urdf/A2D_120s_dual.urdf | 2391 +++++++++++++++++ .../robot_urdf/A2D_120s_dual_high.urdf | 2391 +++++++++++++++++ .../robot_urdf/A2D_120s_fixed.urdf | 2391 +++++++++++++++++ .../robot_urdf/A2D_120s_fixed_cam.urdf | 2391 +++++++++++++++++ .../robot_urdf/A2D_120s_horizon.urdf | 2391 +++++++++++++++++ .../robot_urdf/A2D_120s_left.urdf | 2391 +++++++++++++++++ .../robot_urdf/Franka_120s.urdf | 877 ++++++ runners/data_generator.py | 15 +- runners/task_generator.py | 4 +- task_gen_dependencies/object.py | 6 +- task_gen_dependencies/task_generate.py | 22 +- 21 files changed, 17866 insertions(+), 679 deletions(-) create mode 100644 data_gen_dependencies/robot_urdf/A2D_120s.urdf create mode 100644 data_gen_dependencies/robot_urdf/A2D_120s_dual.urdf create mode 100644 data_gen_dependencies/robot_urdf/A2D_120s_dual_high.urdf create mode 100644 data_gen_dependencies/robot_urdf/A2D_120s_fixed.urdf create mode 100644 data_gen_dependencies/robot_urdf/A2D_120s_fixed_cam.urdf create mode 100644 data_gen_dependencies/robot_urdf/A2D_120s_horizon.urdf create mode 100644 data_gen_dependencies/robot_urdf/A2D_120s_left.urdf create mode 100644 data_gen_dependencies/robot_urdf/Franka_120s.urdf diff --git a/configs/generate_data_config.yaml b/configs/generate_data_config.yaml index bea32e6..d804018 100644 --- a/configs/generate_data_config.yaml +++ b/configs/generate_data_config.yaml @@ -4,4 +4,5 @@ runner: root_dir: "workspace" generate: input_data_root: "/home/ubuntu/Projects/docker_isaac_sim/input/data" - input_target_task_templates_path: "workspace/divide_task/template_targets/task_template_target_0.json" + input_target_task_path: "workspace/generate_task/generate_result_path.json" + server_url: "localhost:50051" diff --git a/data_gen_dependencies/action/place.py b/data_gen_dependencies/action/place.py index 52b8c41..be8fcd9 100644 --- a/data_gen_dependencies/action/place.py +++ b/data_gen_dependencies/action/place.py @@ -19,12 +19,12 @@ class PlaceStage(StageTemplate): 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) + #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) + #print(transform_up) if pre_place_direction == "x": transform_up[0,3] = self.pre_transform_up transform_up[2,3] = 0.02 diff --git a/data_gen_dependencies/action/slide.py b/data_gen_dependencies/action/slide.py index 96f4ea8..134c5fa 100644 --- a/data_gen_dependencies/action/slide.py +++ b/data_gen_dependencies/action/slide.py @@ -39,7 +39,7 @@ class SlideStage(StageTemplate): 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) + #print(pos_diff, succ) if succ: self.step_id += 1 return succ diff --git a/data_gen_dependencies/base_agent.py b/data_gen_dependencies/base_agent.py index f47632b..304388d 100644 --- a/data_gen_dependencies/base_agent.py +++ b/data_gen_dependencies/base_agent.py @@ -4,7 +4,7 @@ import time import numpy as np from scipy.spatial.transform import Rotation as R, Slerp -from data_gen_dependencies.robot import IsaacSimRpcRobot +from data_gen_dependencies.omni_robot import IsaacSimRpcRobot class BaseAgent(object): @@ -14,19 +14,6 @@ class BaseAgent(object): def reset(self): self.robot.reset() - # self.base_camera = "/World/Top_Camera" - # self.robot.base_camera = "/World/Top_Camera" - # self.data_keys = { - # "camera": { - # "camera_prim_list": [self.base_camera], - # "render_depth": True, - # "render_semantic": True, - # }, - # "pose": [self.base_camera], - # "joint_position": True, - # "gripper": True, - # } - # self.get_observation() def add_object(self, object_info: dict): name = object_info["object_id"] @@ -54,9 +41,6 @@ class BaseAgent(object): scale = np.array([object_info["scale"]] * 3) material = "general" if "material" not in object_info else object_info["material"] mass = 0.01 if "mass" not in object_info else object_info["mass"] - # if "materialOptions" in object_info: - # if "transparent" in object_info["materialOptions"]: - # material="Glass" self.robot.client.add_object( usd_path=usd_path, prim_path="/World/Objects/%s" % name, @@ -75,9 +59,6 @@ class BaseAgent(object): def generate_layout(self, task_file, init=True): with open(task_file, "r") as f: task_info = json.load(f) - - # init_position = task_info["init_position"] if "init_position" in task_info else [0] * 39 - # self.robot.client.set_joint_positions(init_position) if init: for object_info in task_info["objects"]: if "box" not in object_info["name"]: @@ -122,7 +103,7 @@ class BaseAgent(object): return True def start_recording(self, task_name, camera_prim_list): - print(camera_prim_list) + #print(camera_prim_list) self.robot.client.start_recording( task_name=task_name, fps=30, diff --git a/data_gen_dependencies/client.py b/data_gen_dependencies/client.py index 43f879a..89828a4 100644 --- a/data_gen_dependencies/client.py +++ b/data_gen_dependencies/client.py @@ -30,7 +30,7 @@ import time import json import pinocchio import os - +from pyboot.utils.log import Log # 目前代码中所有的旋转角度都以角度为单位 class Rpc_Client: @@ -43,12 +43,12 @@ class Rpc_Client: grpc.channel_ready_future(self.channel).result(timeout=5) self.robot_urdf = robot_urdf except grpc.FutureTimeoutError as e: - print(f"Failed to connect to gRPC server[{i}] ({client_host}): {e}", flush=True) + Log.warning(f"Failed to connect to gRPC server[{i}] ({client_host}): {e}") time.sleep(3) if i >= 599: raise e except grpc.RpcError as e: - print(f"Failed to connect to gRPC server[{i}] ({client_host}): {e}", flush=True) + Log.warning(f"Failed to connect to gRPC server[{i}] ({client_host}): {e}") time.sleep(3) if i >= 599: raise e @@ -71,7 +71,7 @@ class Rpc_Client: "target_pose": target_pose } _frame_state: str = json.dumps(frame_state) - print(_frame_state) + #print(_frame_state) req.frame_state = _frame_state response = stub.SetFrameState(req) return response @@ -423,7 +423,7 @@ class Rpc_Client: else: joint_positions.append(joint_data[name]) joint_positions = np.array(joint_positions) - print(joint_positions) + #print(joint_positions) pinocchio.forwardKinematics(model, data, joint_positions) J = pinocchio.computeFrameJacobian(model, data, joint_positions, joint_id) manip = np.sqrt(np.linalg.det(np.dot(J, J.T))) @@ -552,7 +552,7 @@ class Rpc_Client: stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel) req = sim_observation_service_pb2.SetMaterailReq() for mat in material_info: - print(mat) + #print(mat) mat_info = sim_observation_service_pb2.MaterialInfo() mat_info.object_prim = mat["object_prim"] mat_info.material_name = mat["material_name"] @@ -567,7 +567,7 @@ class Rpc_Client: stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel) req = sim_observation_service_pb2.SetLightReq() for light in light_info: - print(light) + #print(light) light_cfg = sim_observation_service_pb2.LightCfg() light_cfg.light_type = light["light_type"] light_cfg.light_prim = light["light_prim"] @@ -579,384 +579,3 @@ class Rpc_Client: req.lights.append(light_cfg) response = stub.SetLight(req) return (response) - - -# 调用示例 -def run(): - rpc_client = Rpc_Client(client_host="localhost:50051") - try: - while True: - send_msg = input("input Command: ") - if send_msg == "30": - result = rpc_client.DrawLine( - point_list_1=[[np.random.uniform(-5, 5), np.random.uniform(-5, 5), np.random.uniform(-5, 5)]], - point_list_2=[[np.random.uniform(-5, 5), np.random.uniform(-5, 5), np.random.uniform(-5, 5)]], - colors=[[np.random.uniform(0, 1), np.random.uniform(0, 1), np.random.uniform(0, 1)]], - sizes=[np.random.uniform(0, 1)], - name="test" - ) - if send_msg == "3111": - result = rpc_client.ClearLine(name="test") - if send_msg == "point": - rpc_client.SetTargetPoint([0, 1, 1]) - if send_msg == "test_mat": - rpc_client.SetMaterial([{ - "object_prim": "/World/huxing/a_keting/yangbanjian_A_016", - "material_name": "Ash", - "material_path": "materials/wood/Ash" - }, - { - "object_prim": "/World/huxing/a_Base/Floor_003", - "material_name": "Ash", - "material_path": "materials/wood/Ash" - - }]) - rpc_client.SetLight([{ - "light_type": "Distant", - "light_prim": "/World/huxing/DistantLight", - "light_temperature": 2000, - "light_intensity": 1000, - "rotation": [1, 0.5, 0.5, 0.5], - "texture": "" - }, - { - "light_type": "Dome", - "light_prim": "/World/DomeLight", - "light_temperature": 6500, - "light_intensity": 1000, - "rotation": [1, 0, 0, 0], - "texture": "materials/hdri/abandoned_hall_01_4k.hdr" - }, - { - "light_type": "Rect", - "light_prim": "/World/RectLight", - "light_temperature": 4500, - "light_intensity": 1000, - "rotation": [1, 0, 0, 0], - "texture": "" - }, - { - "light_type": "Rect", - "light_prim": "/World/RectLight_01", - "light_temperature": 8500, - "light_intensity": 1000, - "rotation": [1, 0, 0, 0], - "texture": "" - }]) - - if send_msg == "908": - pose_1 = [1, 1, 1], [1, 0, 0, 0] - pose_2 = [1, 0, 1], [1, 0, 0, 0] - trajecory_list = [pose_1, pose_2] * 10 - result = rpc_client.SetTrajectoryList(trajecory_list, True) - if send_msg == "31": - target_poses = [] - for i in range(1): - # x = np.random.uniform(0.3, 0.7) - # y = np.random.uniform(-0.3, 0.3) - # z = np.random.uniform(0.96, 1.2) - # pose = { - # "position": [x,y,z], - # "rotation": [1,0,0,0] - # } - x = 0.57597359649470348 - y = -0.45669529659303565 - z = 1.0198275517174573 - rw = 0.066194084385260935 - rx = 0.70713063274436749 - ry = 0.70071350186850612 - rz = 0.0677141028599091 - pose = { - "position": [x, y, z], - "rotation": [rw, rx, ry, rz] - } - target_poses.append(pose) - - result = rpc_client.GetIKStatus(target_poses=target_poses, is_right=False) - print(result) - if send_msg == "1": - # 相机 - # A2W - rpc_client.capture_frame(camera_prim_path="/World/Raise_A2/base_link/Head_Camera") - result = rpc_client.capture_semantic_frame(camera_prim_path="/World/Raise_A2/base_link/Head_Camera") - print(result) - if send_msg == "2": - # 臂 - x = np.random.uniform(0.3, 0.7) - y = np.random.uniform(-0.3, 0) - z = np.random.uniform(0.1, 0.5) - rpc_client.moveto(target_position=[x, y, z], target_quaternion=np.array([0.0, -0.0, -1.0, 0.0]), - arm_name="left", is_backend=True, ee_interpolation=True) - if send_msg == "20": - result = rpc_client.MultiPlan(robot_name="left", target_poses=[ - [[0.5169683509992052, 0.1313259611510117, 1.1018942820728093], - [0.40020943, 0.57116637, 0.69704651, -0.16651593]], - [[0.5610938560120418, 0.048608636026916924, 1.0269891277236924], - [0.40020943, 0.57116637, 0.69704651, -0.16651593]], - [[0.5610938560120418, 0.048608636026916924, 1.2269891277236924], - [0.40020943, 0.57116637, 0.69704651, -0.16651593]] - ]) - print(result[0]) - - if send_msg == "44": - import time - # test cabinet - rpc_client.InitRobot("A2D_120s_horizon.json", "A2D.usd", "omnimanip_Simple_Room_01/simple_room.usd", - init_position=[-0.4, 0, -0.55]) - rpc_client.reset() - time.sleep(1) - rpc_client.DetachObj() - rpc_client.reset() - time.sleep(1) - rpc_client.add_object( - usd_path="objects/guanglun/storagefurniture/storagefurniture011/Storagefurniture011.usd", - prim_path="/World/Objects/storagefurniture", label_name="test_storagefurniture", - target_position=np.array([ - 0.64, - -0.3, - 0.40]), target_quaternion=np.array([0., 0., 0.70711, 0.70711]), - target_scale=np.array([1, 1, 1]), color=np.array([1, 0, 1]), - material="Plastic", mass=0.01) - time.sleep(1) - rpc_client.set_gripper_state(gripper_command="open", is_right=True, opened_width=1) - - target_poses = [] - start_time = time.time() - for i in range(50): - x = np.random.uniform(0, 0.2) - y = np.random.uniform(-0.3, 0.3) - z = np.random.uniform(0.96, 1.2) - pose = { - "position": [x, y, z], - "rotation": [1, x, y, z] - } - target_poses.append(pose) - result = rpc_client.GetIKStatus(target_poses=target_poses, is_right=False) - result = rpc_client.GetIKStatus(target_poses=target_poses, is_right=True) - rpc_client.DetachObj() - print("open") - rpc_client.get_object_pose("/World/Objects/storagefurniture/Storagefurniture011_Handle1") - rpc_client.get_object_pose("/World/Objects/storagefurniture/Storagefurniture011_Handle2") - obj_pose = rpc_client.get_object_pose("/World/Objects/storagefurniture/Storagefurniture011_Handle3") - obj_xyz = np.array( - [obj_pose.object_pose.position.x, obj_pose.object_pose.position.y, obj_pose.object_pose.position.z]) - ee_pose = rpc_client.GetEEPose(is_right=True) - ee_quat = np.array( - [ee_pose.ee_pose.rpy.rw, ee_pose.ee_pose.rpy.rx, ee_pose.ee_pose.rpy.ry, ee_pose.ee_pose.rpy.rz]) - goal_xyz = obj_xyz.copy() - goal_xyz[0] = goal_xyz[0] + 0.3 - goal_xyz[2] = goal_xyz[2] + 0.55 - print(goal_xyz) - rpc_client.moveto( - target_position=goal_xyz, - target_quaternion=ee_quat, - arm_name="left", - is_backend=False, - ee_interpolation=False, - distance_frame=0.001) - obj_pose = rpc_client.get_object_pose("/World/Objects/storagefurniture/Storagefurniture011_Handle3") - obj_xyz = np.array( - [obj_pose.object_pose.position.x, obj_pose.object_pose.position.y, obj_pose.object_pose.position.z]) - ee_pose = rpc_client.GetEEPose(is_right=True) - ee_quat = np.array( - [ee_pose.ee_pose.rpy.rw, ee_pose.ee_pose.rpy.rx, ee_pose.ee_pose.rpy.ry, ee_pose.ee_pose.rpy.rz]) - goal_xyz = obj_xyz.copy() - rpc_client.get_object_pose("/World/Objects/storagefurniture/Storagefurniture011_Handle1") - rpc_client.get_object_pose("/World/Objects/storagefurniture/Storagefurniture011_Handle2") - rpc_client.moveto( - target_position=goal_xyz, - target_quaternion=ee_quat, - arm_name="left", - is_backend=True, - ee_interpolation=False, - distance_frame=0.001) - rpc_client.set_gripper_state(gripper_command="close", is_right=True, opened_width=0.02) - time.sleep(1) - obj_pose = rpc_client.get_object_pose("/World/Objects/storagefurniture/Storagefurniture011_Handle3") - obj_xyz = np.array( - [obj_pose.object_pose.position.x, obj_pose.object_pose.position.y, obj_pose.object_pose.position.z]) - ee_pose = rpc_client.GetEEPose(is_right=True) - ee_quat = np.array( - [ee_pose.ee_pose.rpy.rw, ee_pose.ee_pose.rpy.rx, ee_pose.ee_pose.rpy.ry, ee_pose.ee_pose.rpy.rz]) - goal_xyz = obj_xyz.copy() - goal_xyz[0] = goal_xyz[0] - 0.1 - rpc_client.moveto( - target_position=goal_xyz, - target_quaternion=ee_quat, - arm_name="left", - is_backend=True, - ee_interpolation=False, - distance_frame=0.001) - if send_msg == "21": - rpc_client.MultiMove(robot_name="left", plan_index=0) - if send_msg == "22": - rpc_client.MultiMove(robot_name="left", plan_index=1) - if send_msg == "23": - rpc_client.MultiMove(robot_name="left", plan_index=2) - if send_msg == "24": - rpc_client.start_recording() - - if send_msg == "3": - # 关节 - joint_states = rpc_client.get_joint_positions().states - joint_datas = {} - for joint in joint_states: - joint_datas[joint.name] = joint.position - result = rpc_client.GetManipulability(joint_datas) - print(result) - if send_msg == "4": - # 手 - rpc_client.set_gripper_state(gripper_command="open", is_right=True, opened_width=0.1) - if send_msg == "5": - # 物体 - - import time - # rpc_client.reset() - rpc_client.add_object(usd_path="Collected_cabinet_000/cabinet_000.usd", - prim_path="/World/Objects/cabinet", label_name="test_cabinet", - target_position=np.array([.8, 0., -0.1]), - target_quaternion=np.array([0., 0., 0.70711, 0.70711]), - target_scale=np.array([1, 1, 1]), color=np.array([1, 0, 1]), - material="Plastic", mass=0.01) - time.sleep(1) - result = rpc_client.get_object_joint("/World/Objects/cabinet") - print(result) - # x = np.random.uniform(0.2, 0.64) - # y = np.random.uniform(-0.3, 0.3) - # rpc_client.add_object(usd_path="genie3D/09.usd", - # prim_path="/World/Objects/object8", label_name="test7", - # target_position=np.array([np.random.uniform(0.2, 0.64),np.random.uniform(-0.3, 0.3),1.2]), target_quaternion=np.array([0.0,-0.0,-1.0, 0.0]), target_scale=np.array([0.01,0.01,0.01]), color=np.array([1,1,0]), material="Brass", mass=1) - - # rpc_client.add_object(usd_path="genie3D/02/02.usd", - # prim_path="/World/Objects/object2", label_name="test2", - # target_position=np.array([np.random.uniform(0.2, 0.64), np.random.uniform(-0.3, 0.3),1.2]), target_quaternion=np.array([0.0,-0.0,-1.0, 0.0]), target_scale=np.array([0.01,0.01,0.01]), color=np.array([1,1,1])) - # rpc_client.get_object_pose(prim_path="/World/Xform/box_place") - - # rpc_client.add_object(usd_path="genie3D/06/06.usd", - # prim_path="/World/Objects/object6", label_name="test6", - # target_position=np.array([np.random.uniform(0.2, 0.64), np.random.uniform(-0.3, 0.3),1.2]), target_quaternion=np.array([0.0,-0.0,-1.0, 0.0]), target_scale=np.array([0.01,0.01,0.01]), color=np.array([1,0,0])) - # rpc_client.get_object_pose(prim_path="/World/Xform/box_pick_00") - - # rpc_client.add_object(usd_path="genie3D/01/01.usd", - # prim_path="/World/Objects/object1", label_name="test1", - # target_position=np.array([np.random.uniform(0.2, 0.64), np.random.uniform(-0.3, 0.3),1.2]), target_quaternion=np.array([0.0,-0.0,-1.0, 0.0]), target_scale=np.array([0.01,0.01,0.01]), color=np.array([0,0,1])) - # result = rpc_client.get_object_pose(prim_path="/World/Raise_A2_W_T1/head_link/visuals") - if send_msg == "6": - # 获取全部信息 - rpc_client.get_joint_positions() - if send_msg == "7": - rpc_client.set_gripper_state(gripper_command="close", is_right=True, opened_width=0.02) - if send_msg == "8": - result = rpc_client.get_observation( - data_keys={ - 'camera': { - 'camera_prim_list': [ - '/World/Raise_A2/base_link/Head_Camera' - ], - 'render_depth': True, - 'render_semantic': True - }, - 'pose': [ - '/World/Raise_A2/base_link/Head_Camera' - ], - 'joint_position': True, - 'gripper': True - } - ) - print(result["camera"]) - if send_msg == "9": - rpc_client.start_recording(data_keys={ - 'camera': { - 'camera_prim_list': [ - '/World/Raise_A2/base_link/Head_Camera' - ], - 'render_depth': True, - 'render_semantic': True - }, - 'pose': [ - '/World/Raise_A2/base_link/Head_Camera' - ], - 'joint_position': True, - 'gripper': True - }, fps=30, task_name="test" - - ) - time.sleep(1) - rpc_client.stop_recording() - rpc_client.SendTaskStatus(False) - if send_msg == "10": - rpc_client.stop_recording() - if send_msg == "11": - rpc_client.reset() - if send_msg == "13": - result = rpc_client.Exit() - print(result) - - if send_msg == '112': - rpc_client.InitRobot(robot_cfg="Franka.json", robot_usd="Franka/franka.usd", - scene_usd="Pick_Place_Franka_Yellow_Table.usd") - - if send_msg == '113': - position = [1.2, 0., 1.2] - rotation = [0.61237, 0.35355, 0.35355, 0.61237] - rotation = [0.65328, 0.2706, 0.2706, 0.65328] - width = 640 - height = 480 - rpc_client.AddCamera("/World/Sensors/Head_Camera", position, rotation, width, height, 18.14756, 20.955, - 15.2908, False) - response = rpc_client.capture_frame(camera_prim_path="/World/Sensors/Head_Camera") - # cam_info - cam_info = { - "W": response.color_info.width, - "H": response.color_info.height, - "K": np.array( - [ - [response.color_info.fx, 0, response.color_info.ppx], - [0, response.color_info.fy, response.color_info.ppy], - [0, 0, 1], - ] - ), - "scale": 1, - } - # rgb - # rgb = np.frombuffer(response.color_image.data, dtype=np.uint8).reshape(cam_info["H"], cam_info["W"], 4)[ - # :, :, :3 - # ] - # import cv2 - # cv2.imwrite("head_img.png", rgb) - - if send_msg == '114': - position = [0.05, 0., 0.] - rotation = [0.06163, 0.70442, 0.70442, 0.06163] - width = 640 - height = 480 - rpc_client.AddCamera("/panda/panda_hand/Hand_Camera_1", position, rotation, width, height, 18.14756, - 20.955, 15.2908, True) - response = rpc_client.capture_frame(camera_prim_path="/panda/panda_hand/Hand_Camera_1") - # cam_info - cam_info = { - "W": response.color_info.width, - "H": response.color_info.height, - "K": np.array( - [ - [response.color_info.fx, 0, response.color_info.ppx], - [0, response.color_info.fy, response.color_info.ppy], - [0, 0, 1], - ] - ), - "scale": 1, - } - # rgb - rgb = np.frombuffer(response.color_image.data, dtype=np.uint8).reshape(cam_info["H"], cam_info["W"], 4)[ - :, :, :3 - ] - import cv2 - cv2.imwrite("hand_img.png", rgb) - - except Exception as e: - print("failed.{}".format(e)) - return False - - -if __name__ == '__main__': - run() \ No newline at end of file diff --git a/data_gen_dependencies/data_generate.py b/data_gen_dependencies/data_generate.py index 95ceb5b..21fbe4e 100644 --- a/data_gen_dependencies/data_generate.py +++ b/data_gen_dependencies/data_generate.py @@ -1,15 +1,16 @@ import json - +from pyboot.utils.log import Log from data_gen_dependencies.omni_robot import IsaacSimRpcRobot from data_gen_dependencies.omniagent import Agent -def generate_data(task_file, client_host, use_recording=True): +def generate_data(task_list, client_host, use_recording=True): + task_file = task_list[0] task = json.load(open(task_file)) robot_cfg = task["robot"]["robot_cfg"] stand = task["robot"]["stand"] robot_position = task["robot"]["init_position"] robot_rotation = task["robot"]["init_rotation"] - scene_usd = task["scene_usd"] + scene_usd = task["scene"]["scene_usd"] robot = IsaacSimRpcRobot(robot_cfg=robot_cfg, scene_usd=scene_usd, client_host=client_host, @@ -19,12 +20,13 @@ def generate_data(task_file, client_host, use_recording=True): render_semantic = False if "render_semantic" in task["recording_setting"]: render_semantic = task["recording_setting"]["render_semantic"] - agent.run(task_file=task_file, + agent.run(task_list=task_list, camera_list=task["recording_setting"]["camera_list"], use_recording=use_recording, workspaces=task['scene']['function_space_objects'], fps=task["recording_setting"]["fps"], render_semantic=render_semantic, ) - print("job done") + + Log.success("finish generating data") robot.client.Exit() \ No newline at end of file diff --git a/data_gen_dependencies/object.py b/data_gen_dependencies/object.py index 276b7d4..b5159e3 100644 --- a/data_gen_dependencies/object.py +++ b/data_gen_dependencies/object.py @@ -178,17 +178,18 @@ class OmniObject: grasp_data['grasp_pose'][:, :3, 3] = translation grasp_data['width'] = width - print(f"Grasp num of {obj.name} after NMS: {grasp_data['grasp_pose'].shape[0]}/{N_grasp}") + #print(f"Grasp num of {obj.name} after NMS: {grasp_data['grasp_pose'].shape[0]}/{N_grasp}") # downsample if grasp_data['grasp_pose'].shape[0] > max_grasps: grasp_num = grasp_data['grasp_pose'].shape[0] index = np.random.choice(grasp_num, max_grasps, replace=False) grasp_data['grasp_pose'] = grasp_data['grasp_pose'][index] grasp_data['width'] = grasp_data['width'][index] - print( - f"Grasp num of {obj.name} after downsample: {grasp_data['grasp_pose'].shape[0]}/{N_grasp}") + #print( + # f"Grasp num of {obj.name} after downsample: {grasp_data['grasp_pose'].shape[0]}/{N_grasp}") else: - print(f"Grasp num of {obj.name}: {grasp_data['grasp_pose'].shape[0]}/{N_grasp}") + pass + #print(f"Grasp num of {obj.name}: {grasp_data['grasp_pose'].shape[0]}/{N_grasp}") action_info[grasp_part] = grasp_data diff --git a/data_gen_dependencies/omni_robot.py b/data_gen_dependencies/omni_robot.py index 513e918..4eedd47 100644 --- a/data_gen_dependencies/omni_robot.py +++ b/data_gen_dependencies/omni_robot.py @@ -275,8 +275,8 @@ class IsaacSimRpcRobot(Robot): target_rotation_matrix, target_position = target_matrix[:3, :3], target_matrix[:3, 3] target_rotation = get_quaternion_from_euler(matrix_to_euler_angles(target_rotation_matrix), order="ZYX") - print(f"target_position is{target_position}") - print(f"target_rotation is{target_rotation}") + #print(f"target_position is{target_position}") + #print(f"target_rotation is{target_rotation}") # import ipdb;ipdb.set_trace() # inverse_local_orientation = [self.init_rotation[0], -1*self.init_rotation[1], -1*self.init_rotation[2], -1*self.init_rotation[3]] # local_position = quat_multiplication(inverse_local_orientation,T-self.init_position) @@ -291,7 +291,7 @@ class IsaacSimRpcRobot(Robot): ).errmsg == "True" ) - print("move!", T, quat_wxyz, state) + #print("move!", T, quat_wxyz, state) elif type == "quat": diff --git a/data_gen_dependencies/omniagent.py b/data_gen_dependencies/omniagent.py index 9f950d5..1818802 100644 --- a/data_gen_dependencies/omniagent.py +++ b/data_gen_dependencies/omniagent.py @@ -1,17 +1,19 @@ # -*- coding: utf-8 -*- + import json import os import pickle -import random import time +import random import numpy as np from pyboot.utils.log import Log -from data_gen_dependencies.transforms import calculate_rotation_matrix -from data_gen_dependencies.manip_solver import load_task_solution, generate_action_stages, split_grasp_stages from data_gen_dependencies.base_agent import BaseAgent +from data_gen_dependencies.manip_solver import load_task_solution, generate_action_stages, split_grasp_stages from data_gen_dependencies.omni_robot import IsaacSimRpcRobot +from data_gen_dependencies.transforms import calculate_rotation_matrix + class Agent(BaseAgent): def __init__(self, robot: IsaacSimRpcRobot): @@ -59,7 +61,7 @@ class Agent(BaseAgent): self.add_object(object_info) time.sleep(2) - self.arm = task_info["arm"] + self.arm = task_info["robot"]["arm"] ''' For A2D: Fix camera rotaton to look at target object ''' task_related_objs = [] @@ -195,142 +197,183 @@ class Agent(BaseAgent): return False return True - def run(self, task_file, camera_list, use_recording, workspaces, fps=10, render_semantic=False): - if not self.check_task_file(task_file): - Log.error("Task file bad: %s" % task_file) - return False - print("Start Task:", task_file) - self.reset() - self.attached_obj_id = None + def run(self, task_list, camera_list, use_recording, workspaces, fps=10, render_semantic=False): + for index, task_file in enumerate(task_list): + if not self.check_task_file(task_file): + Log.error("Task file bad: %s" % task_file) + continue + Log.info("start task: "+ task_file) + self.reset() + self.attached_obj_id = None - # import ipdb;ipdb.set_trace() + # import ipdb;ipdb.set_trace() - self.generate_layout(task_file) - # import ipdb;ipdb.set_trace() - self.robot.open_gripper(id='right') - self.robot.open_gripper(id='left') + self.generate_layout(task_file) + # import ipdb;ipdb.set_trace() + self.robot.open_gripper(id='right') + self.robot.open_gripper(id='left') - self.robot.reset_pose = { - 'right': self.robot.get_ee_pose(ee_type='gripper', id='right'), - 'left': self.robot.get_ee_pose(ee_type='gripper', id='left'), - } - print('Reset pose:', self.robot.reset_pose) + self.robot.reset_pose = { + 'right': self.robot.get_ee_pose(ee_type='gripper', id='right'), + 'left': self.robot.get_ee_pose(ee_type='gripper', id='left'), + } + #print('Reset pose:', self.robot.reset_pose) - task_info = json.load(open(task_file, 'rb')) - stages, objects = load_task_solution(task_info) - objects = self.update_objects(objects) - split_stages = split_grasp_stages(stages) - # import ipdb; ipdb.set_trace() - if use_recording: - self.start_recording(task_name="[%s]" % (os.path.basename(task_file).split(".")[0]), - camera_prim_list=camera_list, fps=fps, - render_semantic=render_semantic) # TODO 录制判断 + task_info = json.load(open(task_file, 'rb')) + stages, objects = load_task_solution(task_info) + objects = self.update_objects(objects) + split_stages = split_grasp_stages(stages) + # import ipdb; ipdb.set_trace() + if use_recording: + self.start_recording(task_name="[%s]" % (os.path.basename(task_file).split(".")[0]), + camera_prim_list=camera_list, fps=fps, + render_semantic=render_semantic) # TODO 录制判断 - stage_id = -1 - substages = None - for _stages in split_stages: - extra_params = _stages[0].get('extra_params', {}) - active_id, passive_id = _stages[0]['active']['object_id'], _stages[0]['passive']['object_id'] - arm = extra_params.get('arm', 'right') - action_stages = generate_action_stages(objects, _stages, self.robot) - if not len(action_stages): - success = False - print('No action stage generated.') - break + stage_id = -1 + success = False + substages = None + for _stages in split_stages: + extra_params = _stages[0].get('extra_params', {}) + active_id, passive_id = _stages[0]['active']['object_id'], _stages[0]['passive']['object_id'] + arm = extra_params.get('arm', 'right') + action_stages = generate_action_stages(objects, _stages, self.robot) + if not len(action_stages): + success = False + print('No action stage generated.') + break - # Execution - success = True + # Execution + success = True - for action, substages in action_stages: - stage_id += 1 - print('>>>> Stage [%d] <<<<' % (stage_id + 1)) - active_id, passive_id = _stages[stage_id]['active']['object_id'], _stages[stage_id]['passive'][ - 'object_id'] - if action in ['reset']: - init_pose = self.robot.reset_pose[arm] - curr_pose = self.robot.get_ee_pose(ee_type='gripper', id=arm) - print(arm) - print(curr_pose) - interp_pose = init_pose.copy() - interp_pose[:3, 3] = curr_pose[:3, 3] + (init_pose[:3, 3] - curr_pose[:3, 3]) * 0.25 - success = self.robot.move_pose(self.robot.reset_pose[arm], type='AvoidObs', arm=arm, block=True) - continue + for action, substages in action_stages: + stage_id += 1 + Log.info(f'start action stage: {action} ({stage_id}/{len(action_stages)})') + active_id, passive_id = _stages[stage_id]['active']['object_id'], _stages[stage_id]['passive'][ + 'object_id'] + if action in ['reset']: + init_pose = self.robot.reset_pose[arm] + curr_pose = self.robot.get_ee_pose(ee_type='gripper', id=arm) + interp_pose = init_pose.copy() + interp_pose[:3, 3] = curr_pose[:3, 3] + (init_pose[:3, 3] - curr_pose[:3, 3]) * 0.25 + success = self.robot.move_pose(self.robot.reset_pose[arm], type='AvoidObs', arm=arm, block=True) + continue - # if action in ['grasp', 'pick']: - # obj_id = substages.passive_obj_id - # if obj_id.split('/')[0] not in self.articulated_objs: - # self.robot.target_object = substages.passive_obj_id + while len(substages): + objects = self.update_objects(objects, arm=arm) - while len(substages): - # get next step actionddd - objects = self.update_objects(objects, arm=arm) + target_gripper_pose, motion_type, gripper_action, arm = substages.get_action(objects) + arm = extra_params.get('arm', 'right') + self.robot.client.set_frame_state(action, substages.step_id, active_id, passive_id, + self.attached_obj_id is not None, arm=arm, + target_pose=target_gripper_pose) - target_gripper_pose, motion_type, gripper_action, arm = substages.get_action(objects) - arm = extra_params.get('arm', 'right') - self.robot.client.set_frame_state(action, substages.step_id, active_id, passive_id, - self.attached_obj_id is not None, arm=arm, - 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) - # execution action - if target_gripper_pose is not None: - self.robot.move_pose(target_gripper_pose, motion_type, arm=arm, block=True) + set_gripper_open = gripper_action == 'open' + set_gripper_close = gripper_action == 'close' + self.robot.client.set_frame_state(action, substages.step_id, active_id, passive_id, + self.attached_obj_id is not None, set_gripper_open, + set_gripper_close, arm=arm, target_pose=target_gripper_pose) + self.robot.set_gripper_action(gripper_action, arm=arm) + if gripper_action == 'open': + time.sleep(1) - set_gripper_open = gripper_action == 'open' - set_gripper_close = gripper_action == 'close' - self.robot.client.set_frame_state(action, substages.step_id, active_id, passive_id, - self.attached_obj_id is not None, set_gripper_open, - set_gripper_close, arm=arm, target_pose=target_gripper_pose) - self.robot.set_gripper_action(gripper_action, arm=arm) - if gripper_action == 'open': - time.sleep(1) + self.robot.client.set_frame_state(action, substages.step_id, active_id, passive_id, + self.attached_obj_id is not None, arm=arm, + target_pose=target_gripper_pose) - self.robot.client.set_frame_state(action, substages.step_id, active_id, passive_id, - self.attached_obj_id is not None, arm=arm, - target_pose=target_gripper_pose) + # check sub-stage completion + objects['gripper'].obj_pose = self.robot.get_ee_pose(ee_type='gripper', id=arm) + objects = self.update_objects(objects, arm=arm) - # check sub-stage completion - objects['gripper'].obj_pose = self.robot.get_ee_pose(ee_type='gripper', id=arm) - objects = self.update_objects(objects, arm=arm) + success = substages.check_completion(objects) + self.robot.client.set_frame_state(action, substages.step_id, active_id, passive_id, + self.attached_obj_id is not None, arm=arm, + target_pose=target_gripper_pose) + if success == False: + # import ipdb;ipdb.set_trace() + self.attached_obj_id = None + Log.error('Failed at sub-stage %d' % substages.step_id) + break - success = substages.check_completion(objects) - self.robot.client.set_frame_state(action, substages.step_id, active_id, passive_id, - self.attached_obj_id is not None, arm=arm, - target_pose=target_gripper_pose) - if success == False: - # import ipdb;ipdb.set_trace() - self.attached_obj_id = None - print('Failed at sub-stage %d' % substages.step_id) - break + # attach grasped object to gripper # TODO avoid articulated objects + if gripper_action == 'close': # TODO 确定是grasp才行！！ + self.attached_obj_id = substages.passive_obj_id + elif gripper_action == 'open': + self.attached_obj_id = None + self.robot.client.set_frame_state(action, substages.step_id, active_id, passive_id, + self.attached_obj_id is not None, arm=arm, + target_pose=target_gripper_pose) - # attach grasped object to gripper # TODO avoid articulated objects - if gripper_action == 'close': # TODO 确定是grasp才行！！ - self.attached_obj_id = substages.passive_obj_id - elif gripper_action == 'open': - self.attached_obj_id = None - self.robot.client.set_frame_state(action, substages.step_id, active_id, passive_id, - self.attached_obj_id is not None, arm=arm, - target_pose=target_gripper_pose) - - # change object position - num_against = substages.extra_params.get('against', 0) - against_area = substages.extra_params.get('against_range', []) - against_type = substages.extra_params.get('against_type', None) - if num_against > 0 and gripper_action == 'open' and action == 'pick' and ( - against_type is not None): - parts = against_type.split('_') - need_move_objects = [passive_id] - if parts[0] == 'move' and parts[1] == 'with': - for workspace in objects: - if parts[2] in workspace: - need_move_objects.append(workspace) - ## 目前就集体向一个方向移动 - offset_y = random.uniform(0, 0.2) - poses = [] - for move_object in need_move_objects: - response = self.robot.client.get_object_pose(f'/World/Objects/{move_object}') + # change object position + num_against = substages.extra_params.get('against', 0) + against_area = substages.extra_params.get('against_range', []) + against_type = substages.extra_params.get('against_type', None) + if num_against > 0 and gripper_action == 'open' and action == 'pick' and ( + against_type is not None): + parts = against_type.split('_') + need_move_objects = [passive_id] + if parts[0] == 'move' and parts[1] == 'with': + for workspace in objects: + if parts[2] in workspace: + need_move_objects.append(workspace) + ## 目前就集体向一个方向移动 + offset_y = random.uniform(0, 0.2) + poses = [] + for move_object in need_move_objects: + response = self.robot.client.get_object_pose(f'/World/Objects/{move_object}') + pos = [response.object_pose.position.x, + response.object_pose.position.y + offset_y, + response.object_pose.position.z] + quat_wxyz = np.array( + [ + response.object_pose.rpy.rw, + response.object_pose.rpy.rx, + response.object_pose.rpy.ry, + response.object_pose.rpy.rz, + ] + ) + object_pose = {} + object_pose["prim_path"] = f'/World/Objects/{move_object}' + object_pose["position"] = pos + object_pose["rotation"] = quat_wxyz + poses.append(object_pose) + print(poses) + self.robot.client.SetObjectPose(poses, []) + self.robot.client.DetachObj() + elif (num_against > 0 and gripper_action == 'open' and action == 'pick') or \ + (num_against > 0 and action == 'place' and gripper_action == 'close'): + # import ipdb;ipdb.set_trace() + x_min, x_max = 999.0, -999.0 + y_min, y_max = 999.0, -999.0 + if against_area: + selected_against_area = random.choice(against_area) + if selected_against_area in workspaces: + position = workspaces[selected_against_area]['position'] + size = workspaces[selected_against_area]['size'] + x_min, x_max = position[0] - size[0] / 2, position[0] + size[0] / 2 + y_min, y_max = position[1] - size[1] / 2, position[1] + size[1] / 2 + x_size, y_size, z_size = objects[passive_id].info['size'] + up_axis = objects[passive_id].info['upAxis'] + axis_mapping = { + 'x': (y_size / 2000.0, z_size / 2000.0), + 'y': (x_size / 2000.0, z_size / 2000.0), + 'z': (x_size / 2000.0, y_size / 2000.0) + } + dimensions = axis_mapping[up_axis[0]] + distance = np.linalg.norm(dimensions) + x_min += distance * 1.5 + x_max -= distance * 1.5 + y_min += distance * 1.5 + y_max -= distance * 1.5 + else: + print("against_range not set") + continue + response = self.robot.client.get_object_pose(f'/World/Objects/{passive_id}') pos = [response.object_pose.position.x, - response.object_pose.position.y + offset_y, - response.object_pose.position.z] + response.object_pose.position.y, + response.object_pose.position.z + 0.02] quat_wxyz = np.array( [ response.object_pose.rpy.rw, @@ -339,121 +382,44 @@ class Agent(BaseAgent): response.object_pose.rpy.rz, ] ) + # import ipdb;ipdb.set_trace() + # if position is close to the gripper, then random position again + while True: + pos[0] = random.uniform(x_min, x_max) + pos[1] = random.uniform(y_min, y_max) + distance = np.linalg.norm( + [pos[0] - target_gripper_pose[0][3], pos[1] - target_gripper_pose[1][3]]) + if distance >= 0.2: + break + poses = [] object_pose = {} - object_pose["prim_path"] = f'/World/Objects/{move_object}' + object_pose["prim_path"] = f'/World/Objects/{passive_id}' object_pose["position"] = pos object_pose["rotation"] = quat_wxyz poses.append(object_pose) - print(poses) - self.robot.client.SetObjectPose(poses, []) - self.robot.client.DetachObj() - elif (num_against > 0 and gripper_action == 'open' and action == 'pick') or \ - (num_against > 0 and action == 'place' and gripper_action == 'close'): - # import ipdb;ipdb.set_trace() - x_min, x_max = 999.0, -999.0 - y_min, y_max = 999.0, -999.0 - if against_area: - selected_against_area = random.choice(against_area) - if selected_against_area in workspaces: - position = workspaces[selected_against_area]['position'] - size = workspaces[selected_against_area]['size'] - x_min, x_max = position[0] - size[0] / 2, position[0] + size[0] / 2 - y_min, y_max = position[1] - size[1] / 2, position[1] + size[1] / 2 - x_size, y_size, z_size = objects[passive_id].info['size'] - up_axis = objects[passive_id].info['upAxis'] - axis_mapping = { - 'x': (y_size / 2000.0, z_size / 2000.0), - 'y': (x_size / 2000.0, z_size / 2000.0), - 'z': (x_size / 2000.0, y_size / 2000.0) - } - dimensions = axis_mapping[up_axis[0]] - distance = np.linalg.norm(dimensions) - x_min += distance * 1.5 - x_max -= distance * 1.5 - y_min += distance * 1.5 - y_max -= distance * 1.5 - else: - print("against_range not set") - continue - response = self.robot.client.get_object_pose(f'/World/Objects/{passive_id}') - pos = [response.object_pose.position.x, - response.object_pose.position.y, - response.object_pose.position.z + 0.02] - quat_wxyz = np.array( - [ - response.object_pose.rpy.rw, - response.object_pose.rpy.rx, - response.object_pose.rpy.ry, - response.object_pose.rpy.rz, - ] - ) - # import ipdb;ipdb.set_trace() - # if position is close to the gripper, then random position again - while True: - pos[0] = random.uniform(x_min, x_max) - pos[1] = random.uniform(y_min, y_max) - distance = np.linalg.norm( - [pos[0] - target_gripper_pose[0][3], pos[1] - target_gripper_pose[1][3]]) - if distance >= 0.2: - break - poses = [] - object_pose = {} - object_pose["prim_path"] = f'/World/Objects/{passive_id}' - object_pose["position"] = pos - object_pose["rotation"] = quat_wxyz - poses.append(object_pose) - print(poses) - self.robot.client.SetObjectPose(poses, []) - self.robot.client.DetachObj() - # if num_against > 0 and action == 'place' and gripper_action == 'close': - # offset = random.uniform(-0.1, 0.1) - # response = self.robot.client.get_object_pose('/World/Objects/%s'%passive_id) - # pos = [response.object_pose.position.x, - # response.object_pose.position.y - offset, - # response.object_pose.position.z] - # quat_wxyz = np.array( - # [ - # response.object_pose.rpy.rw, - # response.object_pose.rpy.rx, - # response.object_pose.rpy.ry, - # response.object_pose.rpy.rz, - # ] - # ) - # # import ipdb;ipdb.set_trace() - # poses = [] - # object_pose ={} - # object_pose["prim_path"] = '/World/Objects/' + passive_id - # object_pose["position"] =pos - # object_pose["rotation"] = quat_wxyz - # poses.append(object_pose) - # self.robot.client.SetObjectPose(poses, []) - # index += 1 - # self.robot.client.DetachObj() - + print(poses) + self.robot.client.SetObjectPose(poses, []) + self.robot.client.DetachObj() + if success == False: + self.attached_obj_id = None + break + if self.attached_obj_id is not None: + if self.attached_obj_id.split('/')[0] not in self.articulated_objs: + self.robot.client.DetachObj() + self.robot.client.AttachObj(prim_paths=['/World/Objects/' + self.attached_obj_id], + is_right=arm == 'right') if success == False: - self.attached_obj_id = None break - if self.attached_obj_id is not None: - if self.attached_obj_id.split('/')[0] not in self.articulated_objs: - self.robot.client.DetachObj() - self.robot.client.AttachObj(prim_paths=['/World/Objects/' + self.attached_obj_id], - is_right=arm == 'right') - if success == False: - break - time.sleep(0.5) - if self.attached_obj_id is None: - self.robot.client.DetachObj() - self.robot.client.stop_recording() - # try: - # step_id = substages.step_id if substages is not None and len(substages) else -1 - # except: - # step_id = -1 - step_id = -1 - fail_stage_step = [stage_id, step_id] if success == False else [-1, -1] + time.sleep(0.5) + if self.attached_obj_id is None: + self.robot.client.DetachObj() + self.robot.client.stop_recording() + step_id = -1 + fail_stage_step = [stage_id, step_id] if not success else [-1, -1] - task_info_saved = task_info.copy() - self.robot.client.SendTaskStatus(success, fail_stage_step) - if success: - print(">>>>>>>>>>>>>>>> Success!!!!!!!!!!!!!!!!") + task_info_saved = task_info.copy() + self.robot.client.SendTaskStatus(success, fail_stage_step) + if success: + print(">>>>>>>>>>>>>>>> Success!!!!!!!!!!!!!!!!") return True diff --git a/data_gen_dependencies/robot_urdf/A2D_120s.urdf b/data_gen_dependencies/robot_urdf/A2D_120s.urdf new file mode 100644 index 0000000..455a6c7 --- /dev/null +++ b/data_gen_dependencies/robot_urdf/A2D_120s.urdf @@ -0,0 +1,2391 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/data_gen_dependencies/robot_urdf/A2D_120s_dual.urdf b/data_gen_dependencies/robot_urdf/A2D_120s_dual.urdf new file mode 100644 index 0000000..455a6c7 --- /dev/null +++ b/data_gen_dependencies/robot_urdf/A2D_120s_dual.urdf @@ -0,0 +1,2391 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/data_gen_dependencies/robot_urdf/A2D_120s_dual_high.urdf b/data_gen_dependencies/robot_urdf/A2D_120s_dual_high.urdf new file mode 100644 index 0000000..455a6c7 --- /dev/null +++ b/data_gen_dependencies/robot_urdf/A2D_120s_dual_high.urdf @@ -0,0 +1,2391 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/data_gen_dependencies/robot_urdf/A2D_120s_fixed.urdf b/data_gen_dependencies/robot_urdf/A2D_120s_fixed.urdf new file mode 100644 index 0000000..455a6c7 --- /dev/null +++ b/data_gen_dependencies/robot_urdf/A2D_120s_fixed.urdf @@ -0,0 +1,2391 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/data_gen_dependencies/robot_urdf/A2D_120s_fixed_cam.urdf b/data_gen_dependencies/robot_urdf/A2D_120s_fixed_cam.urdf new file mode 100644 index 0000000..455a6c7 --- /dev/null +++ b/data_gen_dependencies/robot_urdf/A2D_120s_fixed_cam.urdf @@ -0,0 +1,2391 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/data_gen_dependencies/robot_urdf/A2D_120s_horizon.urdf b/data_gen_dependencies/robot_urdf/A2D_120s_horizon.urdf new file mode 100644 index 0000000..455a6c7 --- /dev/null +++ b/data_gen_dependencies/robot_urdf/A2D_120s_horizon.urdf @@ -0,0 +1,2391 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/data_gen_dependencies/robot_urdf/A2D_120s_left.urdf b/data_gen_dependencies/robot_urdf/A2D_120s_left.urdf new file mode 100644 index 0000000..455a6c7 --- /dev/null +++ b/data_gen_dependencies/robot_urdf/A2D_120s_left.urdf @@ -0,0 +1,2391 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/data_gen_dependencies/robot_urdf/Franka_120s.urdf b/data_gen_dependencies/robot_urdf/Franka_120s.urdf new file mode 100644 index 0000000..7567dd8 --- /dev/null +++ b/data_gen_dependencies/robot_urdf/Franka_120s.urdf @@ -0,0 +1,877 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/runners/data_generator.py b/runners/data_generator.py index 7d533e3..8047e6e 100644 --- a/runners/data_generator.py +++ b/runners/data_generator.py @@ -1,11 +1,20 @@ +import json + from pyboot import stereotype from pyboot.runner import Runner - +from pyboot.utils.log import Log from data_gen_dependencies.data_generate import generate_data + @stereotype.runner("data_generator") class DataGenerator(Runner): def __init__(self, config_path: str): super().__init__(config_path) - + self.generate_config = self.config["generate"] + self.input_target_task_path = self.generate_config["input_target_task_path"] + self.input_data_root = self.generate_config["input_data_root"] + self.server_url = self.generate_config["server_url"] + self.target_tasks = json.load(open(self.input_target_task_path, "r")) def run(self): - pass \ No newline at end of file + for task_template_name, task_list in self.target_tasks.items(): + Log.info(f"Generating from template: {task_template_name} | tasks number: {len(task_list)}") + generate_data(task_list, self.server_url) \ No newline at end of file diff --git a/runners/task_generator.py b/runners/task_generator.py index 031f1ce..6f462f3 100644 --- a/runners/task_generator.py +++ b/runners/task_generator.py @@ -29,8 +29,8 @@ class TaskGenerator(Runner): task_template = json.load(open(task_template_path, "r")) generated_tasks_path_list = self.generate_from_template(task_template_name, task_template) Log.success(f"Generated {len(generated_tasks_path_list)} tasks from <{task_template_name}>") - generate_results["task_template_name"] = generated_tasks_path_list - json.dump(generate_results, open(self.output_generate_result_path, "w")) + generate_results[task_template_name] = generated_tasks_path_list + json.dump(generate_results, open(self.output_generate_result_path, "w")) def generate_from_template(self, template_name: str, template: dict) -> List[str]: diff --git a/task_gen_dependencies/object.py b/task_gen_dependencies/object.py index fceb294..a58a16b 100644 --- a/task_gen_dependencies/object.py +++ b/task_gen_dependencies/object.py @@ -181,16 +181,14 @@ class OmniObject: grasp_data['grasp_pose'][:, :3, 3] = translation grasp_data['width'] = width - print(f"Grasp num of {obj.name} after NMS: {grasp_data['grasp_pose'].shape[0]}/{N_grasp}") + #print(f"Grasp num of {obj.name} after NMS: {grasp_data['grasp_pose'].shape[0]}/{N_grasp}") # downsample if grasp_data['grasp_pose'].shape[0]>max_grasps: grasp_num = grasp_data['grasp_pose'].shape[0] index = np.random.choice(grasp_num, max_grasps, replace=False) grasp_data['grasp_pose'] = grasp_data['grasp_pose'][index] grasp_data['width'] = grasp_data['width'][index] - print(f"Grasp num of {obj.name} after downsample: {grasp_data['grasp_pose'].shape[0]}/{N_grasp}") - else: - print(f"Grasp num of {obj.name}: {grasp_data['grasp_pose'].shape[0]}/{N_grasp}") + #print(f"Grasp num of {obj.name} after downsample: {grasp_data['grasp_pose'].shape[0]}/{N_grasp}") action_info[grasp_part] = grasp_data diff --git a/task_gen_dependencies/task_generate.py b/task_gen_dependencies/task_generate.py index fd8ff7f..2ce0868 100644 --- a/task_gen_dependencies/task_generate.py +++ b/task_gen_dependencies/task_generate.py @@ -99,7 +99,7 @@ class OldTaskGenerator: def __init__(self, task_template, data_root): self.data_root = data_root self.init_info(task_template) - self.task_template = task_template + def _load_json(self, relative_path): with open(os.path.join(self.data_root, relative_path), 'r') as file: @@ -171,15 +171,15 @@ class OldTaskGenerator: scene_info = task_template["scene"] self.scene_usd = task_template["scene"]["scene_usd"] self.task_template = { - "scene_usd": self.scene_usd, - "arm": arm, + "scene": task_template["scene"], "task_name": task_template["task"], - "robot_id": robot_id, + "robot":{"robot_id": robot_id,"arm": arm}, "stages": task_template['stages'], "object_with_material": task_template.get('object_with_material', {}), "lights": task_template.get('lights', {}), "cameras": task_template.get('cameras', {}), - "objects": [] + "objects": [], + "recording_setting": task_template.get('recording_setting', {}) } constraint = task_template.get("constraints") robot_init_workspace_id = scene_info["scene_id"].split('/')[-1] @@ -232,14 +232,10 @@ class OldTaskGenerator: generated_tasks_path.append(output_file) task_instance['objects'] = [] task_instance['objects'] += self.fix_obj_infos - task_instance['robot'] = { - "init_position" : self.robot_init_pose["position"], - "init_rotation" : self.robot_init_pose["quaternion"], - "robot_cfg": self.robot_cfg, - "stand": self.stand, - } - task_instance['recording_setting'] = self.task_template["recording_setting"] - + task_instance['robot']["init_position"] = self.robot_init_pose["position"] + task_instance['robot']["init_rotation"] = self.robot_init_pose["quaternion"] + task_instance['robot']["robot_cfg"] = self.robot_cfg + task_instance['robot']["stand"] = self.stand flag_failed = False for key in self.layouts: obj_infos = self.layouts[key]()