finish data generate

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"

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

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

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,

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()

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()

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(
+                                #print(
-                                    f"Grasp num of {obj.name} after downsample: {grasp_data['grasp_pose'].shape[0]}/{N_grasp}")
+                                 #   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
 

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_position is{target_position}")
-                print(f"target_rotation is{target_rotation}")
+                #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":
 

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,11 +197,12 @@ class Agent(BaseAgent):
                     return False
         return True
 
-    def run(self, task_file, camera_list, use_recording, workspaces, fps=10, render_semantic=False):
+    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)
-            return False
+                continue
-        print("Start Task:", task_file)
+            Log.info("start task: "+ task_file)
             self.reset()
             self.attached_obj_id = None
 
@@ -214,7 +217,7 @@ class Agent(BaseAgent):
                 '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)
+            #print('Reset pose:', self.robot.reset_pose)
 
             task_info = json.load(open(task_file, 'rb'))
             stages, objects = load_task_solution(task_info)
@@ -227,6 +230,7 @@ class Agent(BaseAgent):
                                      render_semantic=render_semantic)  # TODO 录制判断
 
             stage_id = -1
+            success = False
             substages = None
             for _stages in split_stages:
                 extra_params = _stages[0].get('extra_params', {})
@@ -243,26 +247,18 @@ class Agent(BaseAgent):
 
                 for action, substages in action_stages:
                     stage_id += 1
-                print('>>>>  Stage [%d]  <<<<' % (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)
-                    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
 
-                    # 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):
-                    # get next step actionddd
                         objects = self.update_objects(objects, arm=arm)
 
                         target_gripper_pose, motion_type, gripper_action, arm = substages.get_action(objects)
@@ -271,7 +267,6 @@ class Agent(BaseAgent):
                                                           self.attached_obj_id is not None, arm=arm,
                                                           target_pose=target_gripper_pose)
 
-                    # execution action
                         if target_gripper_pose is not None:
                             self.robot.move_pose(target_gripper_pose, motion_type, arm=arm, block=True)
 
@@ -299,7 +294,7 @@ class Agent(BaseAgent):
                         if success == False:
                             # import ipdb;ipdb.set_trace()
                             self.attached_obj_id = None
-                        print('Failed at sub-stage %d' % substages.step_id)
+                            Log.error('Failed at sub-stage %d' % substages.step_id)
                             break
 
                         # attach grasped object to gripper           # TODO avoid articulated objects
@@ -405,31 +400,6 @@ class Agent(BaseAgent):
                             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()
-
                     if success == False:
                         self.attached_obj_id = None
                         break
@@ -444,12 +414,8 @@ class Agent(BaseAgent):
             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]
+            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)

data_gen_dependencies/robot_urdf/Franka_120s.urdf

@@ -0,0 +1,877 @@
+<?xml version="1.0" ?>
+<robot name="panda" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <link name="base_link"/>
+  <joint name="panda_fixed" type="fixed">
+    <origin rpy="0 0 0.0" xyz="0 0 0.0"/>
+    <parent link="base_link"/>
+    <child link="panda_link0"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+
+  <link name="panda_link0">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link0.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link0.obj"/>
+      </geometry>
+    </collision>
+
+
+  </link>
+  <link name="panda_link1">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link1.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link1.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.00033 -0.02204 -0.04762"/>
+      <mass value="2.797"/>
+      <inertia ixx="0.0156" ixy="0.0" ixz="0.0" iyy="0.01439" iyz="-0.0024" izz="0.005"/>
+    </inertial>
+  </link>
+  <joint name="panda_joint1" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="0 0 0" xyz="0 0 0.333"/>
+    <parent link="panda_link0"/>
+    <child link="panda_link1"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
+  </joint>
+  <link name="panda_link2">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link2.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link2.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.00038 -0.09211 0.01908"/>
+      <mass value="2.542"/>
+      <inertia ixx="0.0166" ixy="0.0" ixz="0.0" iyy="0.0046" iyz="0.0035" izz="0.015"/>
+    </inertial>
+  </link>
+  <joint name="panda_joint2" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
+    <origin rpy="-1.57079632679 0 0" xyz="0 0 0"/>
+    <parent link="panda_link1"/>
+    <child link="panda_link2"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>
+  </joint>
+  <link name="panda_link3">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link3.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link3.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.05152 0.01696 -0.02971"/>
+      <mass value="2.2513"/>
+      <inertia ixx="0.006" ixy="0.009" ixz="0.003" iyy="0.0086" iyz="0.009" izz="0.0065"/>
+    </inertial>
+  </link>
+  <joint name="panda_joint3" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="1.57079632679 0 0" xyz="0 -0.316 0"/>
+    <parent link="panda_link2"/>
+    <child link="panda_link3"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
+  </joint>
+  <link name="panda_link4">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link4.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link4.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.05113 0.05825 0.01698"/>
+      <mass value="2.2037"/>
+      <inertia ixx="0.0156" ixy="0.0" ixz="0.0" iyy="0.01439" iyz="-0.0024" izz="0.005"/>
+    </inertial>
+  </link>
+  <joint name="panda_joint4" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
+    <origin rpy="1.57079632679 0 0" xyz="0.0825 0 0"/>
+    <parent link="panda_link3"/>
+    <child link="panda_link4"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750"/>
+    <!-- something is weird with this joint limit config
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-3.0" upper="0.087" velocity="2.1750"/>  -->
+  </joint>
+  <link name="panda_link5">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link5.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link5.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.00005 0.03730 -0.09280"/>
+      <mass value="2.2855"/>
+      <inertia ixx="0.02297014781" ixy="0.0" ixz="0.0" iyy="0.02095060919" iyz="0.00382345782" izz="0.00430606551"/>
+    </inertial>
+  </link>
+  <joint name="panda_joint5" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="-1.57079632679 0 0" xyz="-0.0825 0.384 0"/>
+    <parent link="panda_link4"/>
+    <child link="panda_link5"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
+  </joint>
+  <link name="panda_link6">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link6.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link6.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.06572 -0.00371 0.00153"/>
+      <mass value="1.353"/>
+      <inertia ixx="0.00087964522" ixy="-0.00021487814" ixz="-0.00011911662" iyy="0.00277796968" iyz="0.00001274322" izz="0.00286701969"/>
+    </inertial>
+  </link>
+  <joint name="panda_joint6" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
+    <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
+    <parent link="panda_link5"/>
+    <child link="panda_link6"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="12" lower="-0.0175" upper="3.7525" velocity="2.6100"/>
+    <!-- <dynamics damping="10.0"/>
+    <limit effort="12" lower="-0.0873" upper="3.0" velocity="2.6100"/> -->
+  </joint>
+  <link name="panda_link7">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link7.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link7.obj"/>
+      </geometry>
+    </collision>
+        <inertial>
+      <origin rpy="0 0 0" xyz="0.00089 -0.00044 0.05491"/>
+      <mass value="0.35973"/>
+      <inertia ixx="0.00019541063" ixy="0.00000165231" ixz="0.00000148826" iyy="0.00019210361" iyz="-0.00000131132" izz="0.00017936256"/>
+    </inertial>
+  </link>
+  <joint name="panda_joint7" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="1.57079632679 0 0" xyz="0.088 0 0"/>
+    <parent link="panda_link6"/>
+    <child link="panda_link7"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
+  </joint>
+
+  <link name="panda_link8"/>
+  <joint name="panda_joint8" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0.107"/>
+    <parent link="panda_link7"/>
+    <child link="panda_link8"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <joint name="panda_hand_joint" type="fixed">
+    <parent link="panda_link8"/>
+    <child link="right_base_link"/>
+
+    <origin rpy="0 0 0.785398163397" xyz="0 0 0"/>
+  </joint>
+
+  <link
+      name="right_base_link">
+      <inertial>
+        <origin
+          xyz="1.23989288637867E-06 3.27564415436087E-07 0.0536186975993762"
+          rpy="0 0 0" />
+        <mass
+          value="0.443105756928829" />
+        <inertia
+          ixx="0.00064200574020552"
+          ixy="-1.51180224931458E-07"
+          ixz="7.24174172220287E-09"
+          iyy="0.00066998312004943"
+          iyz="2.73584170951165E-09"
+          izz="0.000278949393571484" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="package://meshes/crt_120s/base_link.STL" />
+        </geometry>
+        <material
+          name="">
+          <color
+            rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+        </material>
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="package://meshes/crt_120s/base_link.STL" />
+        </geometry>
+      </collision>
+    </link>
+    <link
+      name="right_Left_01_Link">
+      <inertial>
+        <origin
+          xyz="0.00656387870913618 0.024850259757102 -9.40817030196858E-10"
+          rpy="0 0 0" />
+        <mass
+          value="0.0112899386472626" />
+        <inertia
+          ixx="1.96944973967669E-06"
+          ixy="6.42395813223716E-07"
+          ixz="-1.07950451056367E-14"
+          iyy="1.39704780795747E-06"
+          iyz="-1.48209813327181E-14"
+          izz="2.47457179553145E-06" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="package://meshes/crt_120s/Left_01_Link.STL" />
+        </geometry>
+        <material
+          name="">
+          <color
+            rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+        </material>
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="package://meshes/crt_120s/Left_01_Link.STL" />
+        </geometry>
+      </collision>
+    </link>
+    <joint
+      name="right_Left_1_Joint"
+      type="revolute">
+      <origin
+        xyz="-0.032553 0 0.10644"
+        rpy="1.5708 0 3.1416" />
+      <parent
+        link="right_base_link" />
+      <child
+        link="right_Left_01_Link" />
+      <axis
+        xyz="0 0 -1" />
+      <limit
+        lower="0"
+        upper="1"
+        effort="2"
+        velocity="1000" />
+    </joint>
+
+
+    <link
+      name="right_Left_00_Link">
+      <inertial>
+        <origin
+          xyz="0.00656387870913618 0.024850259757102 -9.40817030196858E-10"
+          rpy="0 0 0" />
+        <mass
+          value="0.0112899386472626" />
+        <inertia
+          ixx="1.96944973967669E-06"
+          ixy="6.42395813223716E-07"
+          ixz="-1.07950451056367E-14"
+          iyy="1.39704780795747E-06"
+          iyz="-1.48209813327181E-14"
+          izz="2.47457179553145E-06" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="package://meshes/crt_120s/Left_00_Link.STL" />
+        </geometry>
+        <material
+          name="">
+          <color
+            rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+        </material>
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="package://meshes/crt_120s/Left_00_Link.STL" />
+        </geometry>
+      </collision>
+    </link>
+
+    <joint
+      name="right_Left_0_Joint"
+      type="revolute">
+      <origin
+        xyz="0.01946 0.0129 0"
+        rpy="0 0 0" />
+      <parent
+        link="right_Left_01_Link" />
+      <child
+        link="right_Left_00_Link" />
+      <axis
+        xyz="0 0 -1" />
+      <limit
+        lower="0"
+        upper="1"
+        effort="2"
+        velocity="1000" />
+    </joint>
+
+
+    <link
+      name="right_Left_Support_Link">
+      <inertial>
+        <origin
+          xyz="-0.011970095332009 0.0238338043684485 -1.36624428992904E-06"
+          rpy="0 0 0" />
+        <mass
+          value="0.0140092102116716" />
+        <inertia
+          ixx="4.13674978137002E-06"
+          ixy="9.81573398608352E-07"
+          ixz="-2.87921970465553E-11"
+          iyy="1.1362453186293E-06"
+          iyz="2.94726072168702E-11"
+          izz="4.10226218364235E-06" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="package://meshes/crt_120s/Left_Support_Link.STL" />
+        </geometry>
+        <material
+          name="">
+          <color
+            rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+        </material>
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="package://meshes/crt_120s/Left_Support_Link.STL" />
+        </geometry>
+      </collision>
+    </link>
+    <joint
+      name="right_Left_Support_Joint"
+      type="revolute">
+      <origin
+        xyz="-0.02888 0.04341 0"
+        rpy="0 0 0" />
+      <parent
+        link="right_Left_00_Link" />
+      <child
+        link="right_Left_Support_Link" />
+      <axis
+        xyz="0 0 1" />
+      <limit
+        lower="-1"
+        upper="1"
+        effort="10"
+        velocity="10" />
+    </joint>
+    <link
+      name="right_Left_Pad_Link">
+      <inertial>
+        <origin
+          xyz="1.46688816627348E-09 0.00209017783344978 -8.17280129359337E-17"
+          rpy="0 0 0" />
+        <mass
+          value="0.00332411394949541" />
+        <inertia
+          ixx="5.5867126170785E-07"
+          ixy="2.9966962370347E-20"
+          ixz="1.44978830317617E-23"
+          iyy="1.45407135182096E-07"
+          iyz="-1.60867378777053E-21"
+          izz="4.18250297449997E-07" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="package://meshes/crt_120s/Left_Pad_Link.STL" />
+        </geometry>
+        <material
+          name="">
+          <color
+            rgba="0.384313725490196 0.384313725490196 0.384313725490196 1" />
+        </material>
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="package://meshes/crt_120s/Left_Pad_Link.STL" />
+        </geometry>
+      </collision>
+    </link>
+    <joint
+      name="right_Left_Pad_Joint"
+      type="fixed">
+      <origin
+        xyz="-0.022056 0.044306 0"
+        rpy="0 0 0" />
+      <parent
+        link="right_Left_Support_Link" />
+      <child
+        link="right_Left_Pad_Link" />
+      <axis
+        xyz="0 0 0" />
+    </joint>
+    <link
+      name="right_Left_2_Link">
+      <inertial>
+        <origin
+          xyz="-0.00438952959966343 0.0248815722409523 -2.55135883071891E-05"
+          rpy="0 0 0" />
+        <mass
+          value="0.00803955821311802" />
+        <inertia
+          ixx="1.15079028835042E-06"
+          ixy="1.13853775681911E-07"
+          ixz="-1.79492796413463E-10"
+          iyy="4.68655200337902E-07"
+          iyz="-4.49903912976825E-10"
+          izz="8.47367915692972E-07" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="package://meshes/crt_120s/Left_2_Link.STL" />
+        </geometry>
+        <material
+          name="">
+          <color
+            rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+        </material>
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="package://meshes/crt_120s/Left_2_Link.STL" />
+        </geometry>
+      </collision>
+    </link>
+    <joint
+      name="right_Left_2_Joint"
+      type="revolute">
+      <origin
+        xyz="-0.017 0 0.122"
+        rpy="1.5708 0 3.1416" />
+      <parent
+        link="right_base_link" />
+      <child
+        link="right_Left_2_Link" />
+      <axis
+        xyz="0 0 -1" />
+      <limit
+        lower="-1"
+        upper="1"
+        effort="10"
+        velocity="10" />
+    </joint>
+    <link
+      name="right_Right_2_Link">
+      <inertial>
+        <origin
+          xyz="-0.00438952959968201 0.0248815722409522 -2.5513588307359E-05"
+          rpy="0 0 0" />
+        <mass
+          value="0.00803955821311803" />
+        <inertia
+          ixx="1.15079028835042E-06"
+          ixy="1.13853775681912E-07"
+          ixz="-1.79492796413511E-10"
+          iyy="4.68655200337903E-07"
+          iyz="-4.49903912976901E-10"
+          izz="8.47367915692974E-07" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="package://meshes/crt_120s/Right_2_Link.STL" />
+        </geometry>
+        <material
+          name="">
+          <color
+            rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+        </material>
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="package://meshes/crt_120s/Right_2_Link.STL" />
+        </geometry>
+      </collision>
+    </link>
+    <joint
+      name="right_Right_2_Joint"
+      type="revolute">
+      <origin
+        xyz="0.017 0 0.122"
+        rpy="1.5708 0 0" />
+      <parent
+        link="right_base_link" />
+      <child
+        link="right_Right_2_Link" />
+      <axis
+        xyz="0 0 1" />
+      <limit
+        lower="-1"
+        upper="1"
+        effort="10"
+        velocity="10" />
+    </joint>
+    <link
+      name="right_Right_01_Link">
+      <inertial>
+        <origin
+          xyz="0.00656387870564603 0.0248502597572661 -9.40812230336897E-10"
+          rpy="0 0 0" />
+        <mass
+          value="0.0112899386472626" />
+        <inertia
+          ixx="1.96944973967663E-06"
+          ixy="6.42395813223747E-07"
+          ixz="-1.07950449101923E-14"
+          iyy="1.39704780795753E-06"
+          iyz="-1.48209815851649E-14"
+          izz="2.47457179553146E-06" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="package://meshes/crt_120s/Right_01_Link.STL" />
+        </geometry>
+        <material
+          name="">
+          <color
+            rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+        </material>
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="package://meshes/crt_120s/Right_01_Link.STL" />
+        </geometry>
+      </collision>
+    </link>
+    <joint
+      name="right_Right_1_Joint"
+      type="revolute">
+      <origin
+        xyz="0.032553 0 0.10644"
+        rpy="1.5708 0 0" />
+      <parent
+        link="right_base_link" />
+      <child
+        link="right_Right_01_Link" />
+      <axis
+        xyz="0 0 1" />
+      <limit
+        lower="-1"
+        upper="1"
+        effort="10"
+        velocity="10" />
+    </joint>
+
+    <link
+      name="right_Right_00_Link">
+      <inertial>
+        <origin
+          xyz="0.00656387870564603 0.0248502597572661 -9.40812230336897E-10"
+          rpy="0 0 0" />
+        <mass
+          value="0.0112899386472626" />
+        <inertia
+          ixx="1.96944973967663E-06"
+          ixy="6.42395813223747E-07"
+          ixz="-1.07950449101923E-14"
+          iyy="1.39704780795753E-06"
+          iyz="-1.48209815851649E-14"
+          izz="2.47457179553146E-06" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="package://meshes/crt_120s/Right_00_Link.STL" />
+        </geometry>
+        <material
+          name="">
+          <color
+            rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+        </material>
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="package://meshes/crt_120s/Right_00_Link.STL" />
+        </geometry>
+      </collision>
+    </link>
+
+     <joint
+      name="right_Right_0_Joint"
+      type="revolute">
+      
+      <origin
+        xyz="0.01946 0.0129 0"
+        rpy="0 0 0" />
+      <parent
+        link="right_Right_01_Link" />
+      <child
+        link="right_Right_00_Link" />
+      <axis
+        xyz="0 0 1" />
+      <limit
+        lower="-1"
+        upper="1"
+        effort="10"
+        velocity="10" />
+    </joint>
+
+    <link
+      name="right_Right_Support_Link">
+      <inertial>
+        <origin
+          xyz="-0.011970095112324 0.0238338043682057 -1.36624428997256E-06"
+          rpy="0 0 0" />
+        <mass
+          value="0.0140092102116716" />
+        <inertia
+          ixx="4.13674978136988E-06"
+          ixy="9.81573398608576E-07"
+          ixz="-2.87921970465403E-11"
+          iyy="1.13624531862944E-06"
+          iyz="2.94726072165632E-11"
+          izz="4.10226218364236E-06" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="package://meshes/crt_120s/Right_Support_Link.STL" />
+        </geometry>
+        <material
+          name="">
+          <color
+            rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+        </material>
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="package://meshes/crt_120s/Right_Support_Link.STL" />
+        </geometry>
+      </collision>
+    </link>
+    <joint
+      name="right_Right_Support_Joint"
+      type="revolute">
+      <origin
+        xyz="-0.02888 0.04341 0"
+        rpy="0 0 0" />
+      <parent
+        link="right_Right_00_Link" />
+      <child
+        link="right_Right_Support_Link" />
+      <axis
+        xyz="0 0 1" />
+      <limit
+        lower="-1"
+        upper="1"
+        effort="10"
+        velocity="10" />
+    </joint>
+    <link
+      name="right_Right_Pad_Link">
+      <inertial>
+        <origin
+          xyz="5.43457184440897E-09 0.00209017783344967 -1.21646039264426E-16"
+          rpy="0 0 0" />
+        <mass
+          value="0.00332411394949542" />
+        <inertia
+          ixx="5.58671261707851E-07"
+          ixy="3.06066210315894E-20"
+          ixz="1.70379713307979E-23"
+          iyy="1.45407135182096E-07"
+          iyz="-1.65096420438087E-21"
+          izz="4.18250297449998E-07" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="package://meshes/crt_120s/Right_Pad_Link.STL" />
+        </geometry>
+        <material
+          name="">
+          <color
+            rgba="0.384313725490196 0.384313725490196 0.384313725490196 1" />
+        </material>
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="package://meshes/crt_120s/Right_Pad_Link.STL" />
+        </geometry>
+      </collision>
+    </link>
+    <joint
+      name="right_Right_Pad_Joint"
+      type="fixed">
+      <origin
+        xyz="-0.022056 0.044306 0"
+        rpy="0 0 0" />
+      <parent
+        link="right_Right_Support_Link" />
+      <child
+        link="right_Right_Pad_Link" />
+      <axis
+        xyz="0 0 0" />
+    </joint>
+
+
+
+
+  <link name="ee_link"/>
+
+  <joint name="ee_fixed_joint" type="fixed">
+    <parent link="right_base_link"/>
+    <child link="ee_link"/>
+    <origin rpy="0 0.0 -1.57079632679" xyz="0.0 0. 0.225"/>
+  </joint>
+  <link name="right_gripper">
+    <inertial>
+      <!-- Dummy inertial parameters to avoid link lumping-->
+      <mass value="0.01"/>
+      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </inertial>
+  </link>
+  <joint name="right_gripper" type="fixed">
+    <!--<origin rpy="0 0 2.35619449019" xyz="0 0 0.1"/>-->
+    <origin rpy="0 0 2.35619449019" xyz="0 0 0.207"/>
+    <axis xyz="0 0 1"/>
+    <parent link="panda_link7"/>
+    <!--<parent link="panda_link8"/>-->
+    <child link="right_gripper"/>
+  </joint>
+
+
+</robot>
+
+

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
+        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)

runners/task_generator.py

@@ -29,7 +29,7 @@ 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
+            generate_results[task_template_name] = generated_tasks_path_list
             json.dump(generate_results, open(self.output_generate_result_path, "w"))
 
 

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}")
+                                #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}")
 
                         action_info[grasp_part] = grasp_data
 

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,
+            "scene": task_template["scene"],
-            "arm": arm,
             "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'] = {
+            task_instance['robot']["init_position"] = self.robot_init_pose["position"]
-                "init_position" : self.robot_init_pose["position"],
+            task_instance['robot']["init_rotation"] = self.robot_init_pose["quaternion"]
-                "init_rotation" : self.robot_init_pose["quaternion"],
+            task_instance['robot']["robot_cfg"] = self.robot_cfg
-                "robot_cfg": self.robot_cfg,
+            task_instance['robot']["stand"] = self.stand
-                "stand": self.stand,
-            }
-            task_instance['recording_setting'] = self.task_template["recording_setting"]
-
             flag_failed = False
             for key in self.layouts:
                 obj_infos = self.layouts[key]()